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ABSTRACT 


Accurate track keeping oTauntonomous underwater vehicles is.necessary for the 
autonomous navigation of a vehicle through confined; spaces, and in the presence of 
obstacles and cross-current environments. Uncertainties in the force coefficients and 
environmental disturbances, as well as the required accuracy lead to the need for a robust 
sliding mode control for successful vehicle operations. This thesis investigates the use of 
a cross track error guidance law with a sliding mode compensator and presents results 
based on computer simulations using a nonlinear dynamic model of the Swimmer Delivery 
Vehicle. Steady state errors and stability requirements are evaluated analytically for a 
given current speed and direction, and are confirmed through numerical integrations. The 
use of integral control and disturbance estimation and compensation methods are 
developed in order to achieve the desired steady state accuracy. A leading track control 
monitoring technique is used to eliminate track overshoot during turning and reduce the 
rudder activity. Finally, the effects of measurement noise are evaluated and guidelines are 
developed for suppressing them. 
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I. INTRODUCTION 


A. BACKGROUND 

Autonomousmnderwater vehicles (AUV’S) are generating much interest in the 
U. S. Navy and major private defense corporations. As monetary and budgetary 
constraints dominate the force structure of the armed forces, intelligent unmanned 
underwater vehicles become a highly attractive alternative to manned submarines [Ref. 
lj. The AUV can be downloaded with a myriad of unclassified missions, i.e., 
reconnaissance, ASW, decoy, survey, ocean engineering; for a fraction of the cost of a 
manned submarine for the same missions. In order for the AUV to cany ont these 
missions, the AUV should be able to operate freely in the ocean environment with respect 
to speed, heading and depth. Such operational requirements have to be easily and reliably 
accomplished in the presence of environmental and physical uncertainties. Autopilot 
design becomes then an integral and important aspect of overall AUV design [Refs. 2, 3, 
4 and 5]. 

The autopilot, which controls the AUV with regards to a commanded direction 
and/or depth, is subjugated to a global planner, which monitors and directs the AUV in 
a global sense. All information concerning the environment of the AUV is detected by 
the sensing instrumentation onboard the AUV and sent to the higher level intelligence 
systems, such as the global planner and the autopilot, so that its missions may be carried 
out. The dynamics of underwater vehicles are described by highly complex, nonlinear 
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systems of equations with uncertain coefficients and disturbances that are difficult to 
measure [Refs. 6,7 and 8]. A complete six degree of freedom model for the underwater 
maneuvering of the AUV is utilized in which the hydrodynamic force coefficients are 
taken from previous studies of a swimmer delivery vehicle?[Refs. 9 and 10]. Recently, 
the development of variable structure control in the form of sliding mode control has been 
shown to provide added robustness that is quite remarkable for AUV autopilot design 
[Ref. 11]. Robust control using sliding mode control provides accurate control of 
nonlinear systems despite unmodelled system dynamics, thus making it a highly likely 
prospect for designing the control laws and guidance methods that will govern the 
autopilot function of unmanned vehicles. 

B. OBJECTIVE OF THIS THESIS 

After developing the necessary sliding mode control theory, the objective of this 
thesis is to investigate the use of a cross track error guidance law with a sliding mode 
compensator and to present results based on computer simulations using a nonlinear 
dynamic model of a swimmer delivery vehicle. Various control methods will be 
investigated for use in the sliding mode based cross track error guidance law. In the 
development of one of the control methods, a current observer will be developed and 
shown to work well. This current observer is direly needed because the current will be 
used as a constant disturbance in this control law and must somehow be determined. 
Since the lateral current for each track is used and cannot be measured for every track for 
all times, then the current must be estimated or observed from parameters that can be measured. 
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After different control methods have been developed and their results presented, a 
leading track control monitoring technique will be developed: This technique can be used 
with each of the:control methods presented. It will be shown that this technique will 
automatically initiate the turn onto the leading track, taking into account the 
environmental conditions, with no overshoot and optimal use of the rudder. 

Finally, noise will be introduced into the measurable parameters and the effects will 
be evaluated. Guidelines will then be developed for suppressing the effects of 
measurement noise. From all this, conclusions will be made and recommendations will 
be developed for a highly robust and effective system for controiling the next generation 
of autonomous underwater vehicles under construction at the Naval Postgraduate School 
and elsewhere in private industry'. 

C. THESIS OUTLINE 

In Chapter 2, the sliding plane and gain coefficients to be used as the basis for 
developing an AUV autopilot, using the sliding mode control theory, are developed. The 
equations of motion to be used only in the horizontal plane, or the path keeping aspect 
of the AUV, are presented. 

Chapter 3 develops a straight line track that becomes the reference from which the 
cross track error is measured. The track nomenclature and geometry to regulate the error 
in deviation, or cross track distance, from the nominal straight line track is presented. 

Chapter 4 develops the integral control method. The effects of adding integral 
control to eliminate the steady state error for a single way point and for multiple way 
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points are investigated. A modified integral control method is developed and results are 
presented. 

Chapter 5 develops a disturbance compensation method, for perfect current input, 
and a disturbance estimation and compensation method, for estimated or observed current 
input. Since the lateral current to each track is used as a constant disturbance in the 
control law, then the lateral current must be able to be determined for each track. The 
only way to determinethe lateral current for each track is to develop a:current observer 
using measurable parameters from onboard sensors. This current observer is developed 
and results arc;presented in this chapter. 

Chapter 6 investigates a technique referred to as leading track control monitoring, 
which utilizes the leading track to automatically initiate the turn onto the leading track, 
within the physical constraints of the AUV and taking into account the environmental 
conditions. This technique initiates the turn so as not to overshoot the leading track and 
to optimize the use of the rudder, within the vehicle contstraints. Results of this 
technique arc presented. 

Chapter 7 introduces noise into the measurable parameters and the effects are 
evaluated. Guidelines for suppressing the effects of measurement noise are put forth in 
this chapter. 


II. EQUATIONS OF MOTION AND SLIDING PLANE DEVELOPMENT 

A. INTRODUCTION 

Prior to be^ning a discussion on cross track error guidance control law, an in 
depth overview of the sliding plane and gain coefficients for use in regulating the steady 
state error in deviation from the nominal straight line track needs to be developed. Also, 
a development of the equations of morion used in this research will be conducted. The 
main assumption to be made, at the beginning, is that only the horizontal plane, or the 
steering aspect of the AUV, will be considered throughout this research. This assumption 
is based upon the previous work done on the Line of Sight (LOS) guidance control Jaw 
by Lienard [Ref. 12], where it was established that heading, speed and depth sliding mode 
autopilots could be designed independently. The remaining pan of this chapter will 
develop the equations of motion for the AUV used in this research and will also develop 
the sliding plane and gain coefficients to be used as the basis for developing an AUV 
autopilot using the sliding mode control theory. 

B. EQUATIONS OF MOTION 

Intend of an exact set of equations of motion for a rigid body moving in a fluid, a 
simplified linear set of equations of motion was chosen to be used for control design. 
The full sets of nonlinear equations of morion for the AUV were taken from the work 
done previously at NPS by Boncal [Ref. 2], who used the dynamic model as established 




by Crane, Summey, et al [Ref, 10], as representative'of the SDV Mark 9 vehicle. The 
SDV Mark 9 vehicle is different than the AUV currently being used at NPS, but it 
remains a useful vehicle for the study of dynamics and control issues. Since the current 
NPS AUV does not yet have validated hydrodynamic coefficients, the SDV Mark 9 
vehicle serves our purpose in developing guidance control laws that can apply to the NPS 
AUV or any vehicle of choice. 

It is too time consuming for an onboard computer to try and control ah underwater 
vehicle using an exact set of equations of motion, therefore, a linearized ser of equations 
of motion was developed. By restricting the;motion of the vehicle to the horizontal plane, 
only the equations of motion in the horizontal plane will be developed. In fact, this 
research utilizes the assumptions and equations of motion done previously by Lienard 
[Ref 12], The state space form that can be used for heading control is 

\j/ = r (2.1a) 

v = a n uv + a u ur + jb.u 2 8 
r = a 21 uv + a 22 ur + b 2 u z & 

with a n = -0.04538, a 12 = -0.35119, a 2l = -0.002795, a n = -0.09568, 

b x = 0.011432 and b 2 = -0.04273, upon which the control laws can be based utilizing 


(2.1b) 

(2.1c) 
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sliding mode control theory- To regulate the error in deviation from a nominal straight 
line track, the following equation is introduced] 


and when linearized 


y = vcosy + usin\\r 


( 2 . 2 ) 


y = v + n\jf (no current), "(2-3) 

where y denotes the cross track distance off the nominal track. So, the state spaceform 
to be used for cross track error control, with no current, is 


\j/ = r (2.4a) 

v = a n uv + a l2 ur + b x u 2 8 (2.4b) 

r = a 2l uv + a 22 ur + b 2 u 2 6 (2.4c) 

y = v + m\|/ (2.4d) 

at any nominal u. 

The system equations of (2.1) and (2.4) will be used for the controller design, 
whereas, the equations developed by Lienard [Ref. 12) for the nonlinear steering equations 
will.be used to simulate the AUV in all trial runs. 
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C. SLIDING PLANE AND GAIN COEFFICIENT DEVELOPMENT 

1. Pole Placement Method 

Since the vehicle motion is based on linearized differential? equations of 
motion, then the nature of the:motion of the vehicle can at best be only approximated. 
Now, any control law based on an approximate plant model must be robust enough to 
ensure stability and acceptable transient response characteristics in the presence of 
parameter variations and/or unmodeled dynamics [Ref. 13]. Since the parameters and 
coefficients are valid for the nonlinear model of the SDV Mark 9 vehicle and a new set 
of parameters and coefficients has still to be verified for the NPS AUV, then there will 
definitely exist parameter variations, unmodelled dynamics, and disturbances. Sliding 
mode control laws provide effective and robust ways of controlling uncertain plants. 
Sliding mode control utilizes a high speed switching:control Jaw to drive the plant’s state 
trajectory on to a sliding plane for all subsequent times. The control law will be based 
upon the linear model 

x = [A]x + [b]u ( 2 - 5 > 

where 

x T = [\|/,v,r,y], b T = [0,0:4116,-0.1538,0], u = 5 
and 
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[A] 


0 0 10 

0 -0.2723 -2.1071 0 
0 -0.0168 -0.1538 0 
6 1 0 0 


For the four dimensional system (2.4), the sliding plane is the Euclidean space 


o(x) = 0 where 5,^ + sjc 2 + + sjc 4 = 0, (2.6) 

and'the coefficient s Y is arbitrary. Equation (2.6) can: be written as 


s T x = 0 with s T = [$,, s v s v sj . (2.7) 

Determining s will determine the sliding plane uniquely. The control law has to be able 
to drive system (2.1) onto the sliding plane (2.6) for an arbitrary choice of initial 
conditions. By defining the Lyapunov function 

V(X): - (2.8) 

asymptotic stability of (2t8) is guaranteed provided V(x) is a negative definite function, 
or 

V = ctf = -r| 2 c(;c), 

such that, 


6 = -T] 2 sign(o). (2.9) 

Since o(;e) = s T x, system (2.1) and equation (2.5) can be used to get 
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s T (Ax + bu ) = -r\ 2 sign(c), 


and solving for u: 


u - ~(s T b)~ l s T Ax - T\ 2 (s T b) rl sign(G), 


or, 


u = u + u. (2.10) 

It is important to recognize that the feedback control' law u is composed of two parts. 
The first, 


u = ~(s T by's T Ax 

is a linear feedback law, whereas the second, 


( 2 . 11 ) 


u = ~r\ 2 (s T by l sign(c) (2.12) 

is a nonlinear feedback with its sign toggling between plus and minus according to which 

side of the sliding ; plane the system is located on. Since u has to change its sign as the 

system crosses c(x) = 0, the sliding surface has to be a hyperplane (dimension of one 

less than the state space). It is u which is mainly responsible for driving and keeping 

the system onto the sliding;plane, c(x) = 0 (where u = 0 as well). Provided the gain 

f] 2 has been chosen large enough, u can provide the required robustness due to 
momentary disturbances and unmodelled dynamics without any compromise in stability. 
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The linear feedback law (2:11) is designedisuch that the system has the desired 
dynamics on the sliding plane. Since a(x) = 0, then in this case 

u = u - ~(s T b)~ l s T Ax, 

and the closed loop dynamics of (2.2) are given by 

x = [A - b(s T By l s T A]x (2.13) 

or, 

x = (A - bk)x , ( 2 - 14 ) 

where the gain vectorJc can be found from standard pole placement methods. The closed 

loop dynamics matrix 

A c = A - bk (2.15) 

has eigenvalues specified for desirable response. One of the eigenvalues of A c must be 

specified to be zero. This is consistent with the decomposition in (2.10). The linear 
feedback u provides the desired dynamics on the sliding plane only. Therefore, u has 
no effect in a direction perpendicular to ct(jc) = 0. With A c specified and k computed 
by pole placement, s can be determined from (2.13) and (2.14), 

k = ( s T b)' l s r A , 


and 


s T {A - bk) = 0, 


(2.16a) 


or, 
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(2.16b) 


s T A c = 0. 

Therefore, s is a left eigenvector of A c that conesponds to the zero eigenvalue. With 

this choice of s, the sliding-plane, s T = 0, and the feedback control law (2.10) are 
completely determined. It should be pointed out that,.in applications, the states x 2 , 
x 2 and x A are to be interpreted as errors between the actual values of \|/, v, r, y and- 
their set points. 

There are two problems that arise from using this approach of pole placement 
in finding s. First, there is no guarantee that the eigenvector for s will always be real. 
Second, for multiple input systems, this approach will not work, since more than one pole 
can not be placed at the origin in order to find s reliably. For this research, these two 
problems did not play a major factor, however, other me thods were investigated. 

2. Coordinate Transformation with Pole Placement Method 

An alternate approach that accounts for the two problems stated in the pole 
placement method is to perform a coordinate transformation and to find the corresponding 
transformation matrix [Refs. 14 and 15], 

Define a coordinate transformation, 

y = Tx , 

where T is an orthogonal n by n transformation matrix such that 
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Tb « 


0 


(2.17) 


where/bj is m by m and 0 is (n - m) by m. The number of states is n and thenumber of 
inputs is m. In this research m = 1. To deti?,'<u'. 7 . use the QR factorization of b, 
where b is decomposed into the form 


b = Q 


R. 

lOJ 


(2.18) 


and Q is orthogonal and R is the upper triangular. Now, fromt(2.17) and (2.18), 


T = Q T . 


From-the coordinate transformation, 


)’ = Tx , 


then. 


^ = T T y 


> 


and, 


y = Tx , 

and when substituted into (2.2), a linear model is obtained in the transformed variable y, 


y = TAT r y + Tbu . 


(2.19) 


The sliding condition, 


s T x = 0 , 


becomes 

s T T T y = 0 , 


or 
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C T y = 0 , 

with C = Ts. Performing a partidonon y and G, 


( 2 . 20 ) 


y = 



where yj is o-ie by one and y 2 is (n - i) by one, and 



where C, is one by one and C 2 is (n - 1) by one, so that the state equations in the 
transformed variable become 


y, = A,,; + A 12 y 2 + b x ii (2.21a) 

y 2 = A 2I y, + A 22 y 2 . (2.21b) 


The sliding niane (2.20) now becomes 
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y, + -cly 2 = 0 , 


(2.22) 


with C, normalized to one. For the sliding plane to be completely and uniquely defined, 


then C, needs to be determined. 


Again by defining the Lyapunov function 


V(y) = i.(a(>-)) 2 , 


(2.23) 


asymptotic stability of (2.23) is guaranteed provided V(y) is a negative definite function, 


V(y) = cd = -r| 2 (j(}’) , 


(5 = -T| 2 51g/l(a) . 


(2.24) 


Differentiating the equation for the sliding plane (2.20)-and equating to (2.24), 


>’i + C 2% = -'n 2 5/gn(a) . 

Substituting (2.21a) and (2.21b) into (2.25) and solving for 


(2.25) 


u = u + u , 


A„y, + A 12 y 2 + b x u + C 2 {A 2X y x + A 22 y 2 ) = -resign (a) 


u = -b x '[(/!,, + C 2 A 2i )y x + (A n + C 2 T 4 22 )y 2 ] 


(2.26a) 
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u = -rfb^ sign((f) . (2.26b) 

From the sliding-plane design, it is desired to have a(y) = 0 , which gives u = 0 andn = ii. 

Solving (2.22) for y,, 


* = -C r 2 y 2 . (2.27) 

Equations on the-sliding plane become 

*1 = + ^ + b fi . 

and substitutingTor u from (2.26a), 

j, = -C 2 T (A 21 y, + , 

and substituting once more for y { by differentiating (2.27), 

-C?y 2 = -C 2 (A 2l y t + ; 

a linear combination of the second set 

y 2 = A 2l y, + iVi • 

The (n - 1) independent equations on the sliding plane are 

^2 = ^ 21^1 + ^ 22^2 * 
and substituting for y x from (2.27) 

y 2 = (A n - A 2l C 2 )y 2 . (2.28) 

Again using pole placement of (n - 1) poles, C 2 can be determined and thus the sliding 


plane is uniquely and completely determined. 
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The system matrix in the y equation has rank=(n -1) and is a singular matrix, 
therefore, one pole is already at the origin. Only (n -1) poles need to be determined by 
pole placement and the two problems from the previous pole placement method have been 
resolved. 


3. Linear Quadratic Regulator Coordinate Transformation 

Instead of pole placement to determine the sliding plane and gain coefficients, 
the Linear Quadratic Regulator (LQR) with a coordinate transformation was investigated. 
This LQR method involves minimizing a cost functional in which the integrand is a 

quadratic function of the state x(-) [Refs 14 and 15], such as 


J » If (x T Qx) dt . 

Using the coordinate transformation, y = Tx, 

/ = if y T (TQT r )y dt , 

and by partitioning 


TQT r = 


~Q\\ Qn 

Q 2l Qv 


(2.29) 


the cost functional becomes 

J = 4/ + + ylQj&z + >iQ2i>9 dl ■ 


Now defining 






Q - Q 22 “ ^21^11 ^12 


(2.30a) 


4 * = A* - A 2l Q n ~ l Q l2 
v = y x + QnQiJi . 

a new cost functional, I, is obtained 

/ - if + ' /7 '2„V) dt 

and 

*2 = /*‘y 2 + 4 21 V . 


(2.30b) 


(2.30c) 


(2.31) 


The problem is now to minimize I subject to (2.31). However, in order to 
minimize I, an arbitrary choice of Q' needs to be made. The choice of Q‘ greatly 

influences whether right control or soft control will be obtained, but it provides no easier 
a method to obtain the sliding plane and gain coefficients. For the remainder of this 
research, the pole placement method is chosen to determine the sliding plane and gain 
coefficients with closed loop poles as specified for the particular trial run. 
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m. TRACK DEVELOPMENT 

A. INTRODUCTION 

Now that the foundation in sliding mode control theory has been laid and the 
method for determining the sliding plane and gain coefficients is known, ^straight line 
track needs to be developed. It will be the perpendicular distance off this straight line 
track that will be defined as the cross track distance, y. This cross track distance will be 
the object of the sliding mode control laws so that this cross track distance will be 
controlled to zero. When the cross track distance is zero, then the vehicle is on the 
directed track specified by the global planner. This chapter will develop the track 
nomenclature and geometry in order to regulate the error in deviation, or cross track 
distance, from the nominal straight line track. Also, this chapter will show that, at steady 
state conditions, a steady state error exists in the presence of a current and how the value 
of k r affects the stability of the controller. It will be the elimination of this steady state 

error that the various guidance controls laws to be developed will concern themselves. 

B. NOMINAL STRAIGHT LINE TRACK 

In order to construct the nominal straight line track to be used to measure the cross 
track distance, the global planner needs to input two way points, a stoning point and a 
destination point. For this research, the two way points must be in global coordinates 
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(X,Y) and in terms of ship lengths. The following equations determine the inertial 
position rates of the AUV 

X = U c + ucosy - vsirity 
Y =V c + usin\\r + vco.y\y 

where U c and V c are the absolute current velocities in the global reference frame. The 

angle a, measured from the horizontal, will define the track for the two way points of 
interest. The perpendicular distance y, in local coordinates, will be die cross track 
distance that will be controlled: such that when the cross track distance is zero,.then the 
vehicle is on the desired track. The cross track distance, for this research, will be 
designated, y, and the distance along the track will be designated, x. Both y and x are 
in local coordinates. The current position of the vehicle will be designated by, X and Y, 
both in global coordinates. 

1. Geometry of a Nominal Straight Line 

Figure 1 will be used to develop a nominal straight line track, and it will be 
repeated as the vehicle goes from way point to way point. The equations to transform 
the global coordinates into the local coordinates are 

* = Xcosa + Ysina 
y = Ycosa - Xsina . 

Also, the equations to translate global currents into local currents: are 
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u = £/ cosa + V sina 

c c c 

v = V cosa - U sina . 

c c-- c 

2. Nomenclature of a Nominal Straight Line Track 

The following nomenclature will be used in developing a nominal straight line 
track for this research: 

• y: the perpendicular distance off the track, in local coordinates. 

• x: the distance along the track, in local coordinates. 

• XTIME: desired total time to go from the starting point to the destination 

point, in seconds. 

• UREQ: the speed required to get ffomthe starting point to the destination point in 

the desired time, in ft/sec. 

• x T : the total length of desired track, in feet. 

• (X,Y): the current vehicle position, in global* coordinates. 

• (X d ,Y d )\ the destination way point, in global coordinates and in ship lengths. 

• (X 0 ,Y 0 ): the starting way point,.in global coordinates and in ship lengths. 

• a: the angle measured from the:horizontal to the line between the starting 

point and the destination point. 
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From the geometry, the following parameters will be defined: 


x T = 


Y - Y 

1 D 1 O 

sina 


• a = tan 


rl 


Y - Y 

1 D 1 0 


*o~ * 0j 


y - (Y - Y 0 )cosoc - (X - X 0 )sinot 


• x = (X - X 0 )cosa + (Y - F 0 )sina . 




UREQ = 


XT!ME 


C. STEADY STATE ERROR 

In the presence of a current, it has been observed that a steady state error will 
occur, with no control, for the linearized set of equations of motion for the AUV at steady 
state conditions. The linearized set of equations of motion for the AUV, with no integral 
control, was developed in equation (2.1). To account for the current that is perpendicular 

to the track, v., equation (2.3) is modified to 

y = v + mj/ + v e . (3.1) 

At steady state, 
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\\f = v = r = y = 0 


( 3 . 2 ) 


sothat 


0 = r t (3.3a) 

0 = a n uv s + b 1 u 2 d s ;(3.3b) 

0 = a 2X uv s + b 2 u 2 5 s (3.3c) 


0 = v s + usinty + v c (3.3d) 

where a iv a 2l , b v b 2 and u are as defined in Chapter Two. The subscript s represents 

the value of the variable at steady state. 

Since a n , a 2V b v and b 2 are nonzero, then v f = 8 s = 0. Therefore, 


Vs = ~ sin 


-i 


( ^ 
v 




(3.4) 


and when this is substituted into the sliding plane and rudder equations, a steady state 
error will develop. For rudder control, 


8 = fcjX|/ + k 2 v + k 3 r + k K satsgn( o) (3.5) 

and for the sliding plane, 
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Pi . 

sy t = ---sin 1 
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and 


y, = 


1 


P, 


+ -s t 


sin 


-l 


f \ 
v 


K U J 


( 3 . 12 ) 


Equation (3.12) represents the steady state error in the crossitrack distance that results in 
the presence of a lateral current. This steady state track error can be made smaller by 
increasing the value of the nonlinear gain k n ,but it can never become zero. For very 

large k n , y s is still bounded by 


■»i . _i 

y = — sin 1 


V 

\ u j 


The above analysis is valid if 


a 


satsgn(o) = _ , 

4> 


which requires that | :satsgn(o) | < 1. This requirement yields the necessary critical value 
of k for stability, from (3.10) 


k > k . = 

n cnt 


( \ 


k, sin 


-i 




(3.14) 


If the nonlinear gain is not selected large enough; i.e., if k n < k ent \ then the controller 
cannot guarantee stability. 
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The above analytical results can beconfirmed by numerical" simulations of the full 
nonlinear, six degree of freedom model of the SDV. Closed loop poles on the sliding 
plane were selected at [-0.35,-0.36,-0.37],,with <j> = 0.5 and u = 6;ft/sec. This gives 

fi = 0.9556i{/ - 0.1085v + l,2286r + k n satsgn{p) 15 ) 


o = 2.9805t}f + 0.2199v + 3.4445r + 0.0700y . (3.16) 

Figure 2 shows how the controller works; with no current. The way point is selected at 
(X,Y) = (20,20) ship lengths. In Figure 2 and all subsequent similar figures, the 
following variables are displayed: X vs Y position, rudder angle vs time, the cross track 
error (YLCASE) vs time, the heading \!/ - a (HEAD) vs time, the integral of the cross 
track error (YINTGR) vs time and the sliding surface a (SS2) vs^time. It can be seen 
that the vehicle achieves the desired track with no error. Figures 3 and 4 show how the 
controller works with a current and how the larger the k n , the smaller the steady state 

cross track error. Figure 4 and (3.12) also show that as £„ .gets infinitely large, a steady 

state error will still exist and the rudder will be cycled excessively. The current was 
U e = 0.0, V e = 2.0 ft/sec, which means that v e = 1.4142 ft/sec, and using (3.14), 

k cril = 0.2274. It can be seen that the steady state error, as predicted by (3.12), is in 

accordance with what was obtained through the numerical simulations. Finally, Figure 
5 shows that for k n < k crit , the controller cannot guarantee stability and the controller 

starts to deviate in a linear manner in the presence of a current. 
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Figure 2. Cross Track Error Control with k = 0.5 and V =0 
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Figure 3. Cross Track Error Control with k_ = 1.0 and V = 2 








ITGR (SHIP LENGTHS! r'LCRSE (SHIP LENGTHS) i (SHIP LENGTHS! 














ililTGP iSHIP LENGTHS) iLCRSE (SHIP _ENGTHSi i (SHIP LENGTHS! 




T 






Figure 5. Cross Track Error Control with k R = 0.2 and V c = 2. 
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Results for multiple way points are presented in Figures 6 through 9. The way 
points were selected at (X,Y) = (10,0), (20,5) and (30,5). This corresponds to a lane 
changing maneuver, the change from the original track to a second parallel track. The 
local coordinate system is-rotated every time a way point is reached. The criterion for 
reaching a way point is based on the distance from the way point along the local x 
direction, or target distance. Results for various values of the target distance are shown 
in Figures 6, 7 and 8, where the target distances are 0.5, 2 and 7 vehicle lengths, 
respectively. It can be seen that if the target distance is very small, the vehicle 
overshoots therdesired track with significant rudder activity. On the other hand, if the 
target distance is very large, the vehicle turns in the wrong direction prior to completing 
the turn. The best target distance depends on the turn, vehicle response characteristics, 
and environmental conditions; and in this case it appears that a value of 2 causes 
minimal rudder and track overshoot. 

Finally, the attempted lane changing maneuver in a current V. = 2.0 is shown in 

Figure 9, where the existence of a significant steady state track error is evident. The 
following chapters will explore the use of an integral control method and the use of a 
disturbance estimation and compensation method to control the steady state error in the 
presence of a current, such that the vehicle remains on the desired track. These methods 
of control will utilize the above development of the desired track. Once the desired track 
has been defined, these methods of control will attempt to control the cross track distance 
to zero, so that the AUV will be on the desired track as consistently as possible. 
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Figure 6. Lane Changing Maneuver, Target Distance 0.5 Vehicle Lengths. 
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Figure 7. Lane Changing Maneuver, Target Distance 2 Vehicle Lengths. 
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Figure 9. Lane Changing Maneuver in a Current. 
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IV. INTEGRAL CONTROL METHOD 

A. INTRODUCTION 

In the previous two chapters, the sliding mode control development, along with the 
nominal straight line track and the steady state error in the presence of a current, for a 
cross track error controller, have been developed. The first method to be used to 
eliminate this steady state error is the integral control method. This is the first logical 
choice, since traditionally, integrators have been used to eliminate steady state errors. 
However, in general, as more integrators are added to the system, then the chances 
increase for the system to become unstable due to the poles being added to the system. 
Also, when the integral action is introduced, the linearized equations for the system have 
to be modified, which will be seen in this chapter. This chapter will also show the effects 
of adding integral control to eliminate the steady state error for a single way point and 
for multiple way points, as in Chapter Three. A modified integral control method will 
also be investigated in this chapter with results to show how well the modified method 
works. 

B. INTEGRAL CONTROL METHOD 

Before proceeding with the method of integral control, the linearized system 
equations must be modified. If the cross track error y needs to reach zero at steady state 
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conditions in the presence of constant disturbances, then the state equations are augmented 
by 

y, = y • ( 4 - L ) 

Feedback of y, then brings in the desired integral action. The augmented linear control 
law becomes 


5 = k^ + k 2 v + k 3 r + kj + k n satsgn(o ) , (4.2) 


a = jr,\j/ + j 2 v + s 3 r + sj + sjy, . 

Then at steady state, 

r, = v, = 8, = 0 

and (3.4) still holds with the additional y s = 0 from (4.1). 


(4-3) 


C. STEADY STATE ERROR 


The requirement of b s = 0 and (4.2) with (3.4)'-yield 


f ^ 


-1 < satsgn(o) = — sin -1 
k. 




< i , 


(4-4) 


which establishes the lowest limit, k > k ., with k . given by (3.14). As long as this 


inequality is satisfied, then the integral control method will drive the cross track offset 
y to zero. The closed loop poles on the sliding plane were selected at 
[-0.35,-0.36,-0.37,-0.05], with <{> = 0.5 and u = 6 ft/sec. This gives 
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5 = 1.2948\|/ - 0.0834v + 1.6206r + 0.0080)’ + k n satsgn(<5 ) (4.5) 

a = 3.3118y + 0.2635v + 3.5612r + 0.0948y + 0.0035}-, . (4.6) 

Figure 10 shows how the integral control method works in the presence of a constant 
disturbance, a current. The current was U- = 0.0; V c = 2.0 ft/sec, which means that 

v c = 1.4142 ft/sec for the chosen way point of (X, Y) = (20,20) ship lengths. Using 

(3.14), k crit = 0.3081. Figure 10 was conducted using = 2.0, which is larger than 


k.. From (4.4), 


k \ 

— sin 1 


( \ 


K U J 


= 0.1540 , 


thus the inequality of (4.4) is satisfied; and as seen in Figure 10, the integral control 
method drives the cross track offset y to zero, with some overshoot. As k n is increased, 

the cross track error is brought to zero quicker, but the rudder is cycled much more 
excessively. Figure 11 was conducted with k n = 0.2, which is smaller than k cril , and the 


same current conditions as in Figure 10. From (4.4), 




1 sin" 1 




= 1.5404 , 


thus the inequality of (4.4) is not satisfied and a steady state error developed. As seen 
in Figure 11, if the nonlinear gain is not selected large enough; i.e., if k < k • then the 


integral controller cannot guarantee zero steady state error. In this case, 
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Figure 11. Integral Control with V e = 2 and for k M < k cnl 







satsgn(o) = 1 , 

which means ct > <j), and using 5^ = 0 and (3:4), 



Equation (4.7) yields the steady state cross track error of the integral controller for small 
k n . This was seen in Figure 11, where the integral control design developed a constant 

track deviation, unlike the cross track error controller which was unstable. Using (4.7), 
with the given values, y s - 0.7765, as seen in the YLCASE vs TIME graph of Figure 

11. This unique characteristic of the sliding mode track controller - the existence of a 
■nonzero steady state error - is attributed to the lack of a linear feedback gain in y, in 

(4.2). The term y s appears only in the sliding surface equation (4.3), and if the nonlinear 

gain k n does not possess the necessary strength, it cannot guarantee steady state accuracy. 

■A modified integral control method will be developed later to solve this problem with the 
integral control; method. In the case where the integral controller is operating in the 
environment with no current, Figure 12 shows that the vehicle achieves the desired track 
with zero steady state error. 

Results for multiple way points are presented in Figures 13 and 14. The way points 
were again selected at (X,Y) = (10,0), (20,5) and (30,5), for comparison. The target 
distance for both figures was two vehicle lengths. Figure 13 shows the integral controller 
not having enough time, for the given current, to drive the vehicle onto the desired track. 
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Figure 12. Integral Control with No Current. 
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Figure 14. Lane Changing Maneuver Under Integral Control with No Current 



















The current conditions for Figures 13 and l4 were as in Figure 10 with k n = 2.0 . Figure 
14 shows how well the integral controller works for multiple way points with no current. 

D. MODIFIED INTEGRAL CONTROL 

An alternate design procedure that can eliminate the problem of the existence of a 
nonzero steady state track error, using integral control, is the modified integral: control. 
Consider the linear system 


and the slidingisurface 


x = Ax + bu 


(4.8) 


a - s T x . 

The sliding condition oG < 0 is met by 


(4.9) 


G - - t| 2 sign(a) , (4.10) 

which gives the control law 

u = -(s T b)" l s T Ax - T\ 2 (s T by l sign(c) . (4.11) 

Then, s can be found as a left eigenvector of the closed loop dynamics matrix which 
corresponds to the zero eigenvalue, as developed in Chapter Two. If, instead of (4.10), 

it is required that g<5 < 0 be met by 


G + = -T\ 2 sign(G) ; ^ > 0 , (4.12) 

then the control law becomes 
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(4.13) 


u = - ( s T b)~ x s T (A + £j)x - T[ 2 (s T by 1 sign(<3) , 
and s can be found as a left eigenvector of the closed loop dynamics matrix which 

corresponds; to the eigenvalue Provided 4 is chosen smallenough, (4.12) satisfies 

a "near" sliding condition and a sliding condition in the limit, t —» «>. In this case of 

track control, (4.13) becomes 

8 = (A'j + , )y+(A' 2 + £s 2 ) v+(k 2 +£s 2 )r+(k.+cs A )y +cy, 

+k H satsgn(o) . (4.14) 

Results are presented in Figure 15 for v £ = 1.4142 ft/sec lateral current, £ = 0.1 and 

k n = 0.2. It can be seen that the presence of the £ - term in (4.14) eliminates the steady 
state error that is otherwise present. For k n values higher than k erii , the response 
characteristics of the two integral control laws (4.2) and (4.14) are very similar, as seen 
in Figure 16. Figure 16 used k r = 2.0 and when compared to Figure 15 and Figure 10, 

all figures show similar results for k n > k erir 
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V. DISTURBANCE ESTIMATION AND; COMPENSATION METHOD 

A. INTRODUCTION 

The integral control methods of the previous section will ensure zero steady state 
error, for k n > k crit and for k n < k crit , especially when using the modified integral control 

method. To-improve on the transient response and to achieve the desired steady state, a 
disturbance estimation and compensation can be introduced in the;cross track error design 
of the controller. This chapter will investigate the disturbance estimation and 
compensation method, which formulates the cuitent as a disturbance to be included 
directly into the control law. In this chapter, the disturbance compensation method will 
first be developed with a perfect current input and then the disturbance estimation and 
compensation method will be developed with an estimate of the current using a current 
observer. This method will follow the same development as for the integral control. 
First, a single way point will be investigated with current and then without current, to see 
how well both methods control the AUV onto the desired track. Multiple way points will 
next be investigated; and results will be given to show how well both methods handle 
these multiple way points. This methodology will be followed for the perfect current 
input as well as for the estimated current input. 
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B. DISTURBANCE COMPENSATION METHOD 


The sliding surface (3.6) is modified to 


a = 5,\j/+5 2 v+^ 3 r+j' 4 y+ <j)_+jj sin" 1 

Ic it 


with 8 as in (3.5). At steady state, r s = v s = 8^ = 0 and (3.4) is valid with y s = 0, as 
dictated by (5.1) and 


satsgn(<5 ) = _ . 

If k n < k cril , then the disturbance compensation controller, with the cross track error, 
cannot guarantee stability. The steady state response in such a case is characterized by 


r. = v = 8 = 0 


s s s 


and by 


• i K 

V, = -sm 1 -1 , 


with y s linearly increasing in time with the rate of change given by 


v c • K 
y = -i - sin _1 . 

f u L 


Equation (5.3) is obtained from (3.5) for 8=0 with satsgn{G) = 1. 
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C. STEADY STATE ERROR 

Again the requirement of 8^ = 0 and (4.2) with (3.4) yields (4.4), which establishes 

the lowest limit, k K > k cHt , with k cril givemby (3.14). Again, as long as this inequality 

is satisfied, the disturbance compensation method will drive the cross track offset y to 
zero. The closed loop poles on the slidingjplane were again selected at 
[-0.35, -0.36, -0.37], with (j) = 0.5 and u = 6 ft/sec. This gives 


5 = 0.9556\pr - 0.1085v + 1.2286r + ksatsgn{o) 


(5.5) 


f 


a = 2.9805y+0.2199v+3.4445r+0.0700y+ 


0.4778 


\ 


+2.9805 


sih-'(0.2357). ( 5 . 6 ) 


J 


Figure 17 shows how the disturbance compensation method works in the presence of a 
current, U e = 0.0, V c = 2.0 ft/sec, which means that v, = 1.4142 ft/sec for the chosen 

way point (X,Y) = (20,20) ship lengths. Using (3.14), k ctil = 0.3081, as for the integral 


control method. Figure 17 was conducted using a perfect current input and k = 2.0, 


which is larger than k cril . As seen in Figure 17, the disturbance compensation method 

brings the steady state error to zero with no overshoot and with a quicker response than 
the integral control method. In Figure 17 and all subsequent similar figures for the 
disturbance compensation and the disturbance estimation and compensation methods, the 
following variables are displayed: X vs Y position, rudder angle vs TIME, the cross track 
error (YLCASE) vs TIME, the heading y - a (HEAD) vs TIME, v c (VCUR) vs TIME 
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Figure 17. Disturbance Compensation, V c = 2, k R > k cril 
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and u c (UCUR) vs TIME, for the perfect current input; and v c (VCOOBS) vs TIME and 

u c (UCOOBS) vs TIME, for the estimated current input. Figure 18 shows how the 

disturbance compensation method works in the presence of the same current conditions 
as in Figure 17 for k n = 0.2, which is less than k crit . As seen in Figure 18, the 

disturbance compensation method design gives rise to unstable behavior. 

Results for multiple way points are presented in Figure 19. The way points were 
selectedlat (X,Y) = (10,0), (20,5>and (30,5) ship lengths,.for comparison. Again, the 
target distance was selected at 2 vehicle lengths. Figure 19 Was conducted with the same 
current conditions as tin Figure 17. 

D. CURRENT OBSERVER DEVELOPMENT 

As seen in the previous section, the disturbance compensation method works well 
with absolute knowledge of the current. However, in reality, the current is never 
absolutely known at every location, so a current observer must be developed. A reduced 
order observer can be designed based on y, y and r measurements to estimate the lateral 
current velocity v c and the current velocity along the track u c . The observer design is 
based on-the linear equations (2.1a), (2.1b), (2.1c) and 

y = v c + u\f + v (5.7) 
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Figure 19. Lane Changing Maneuver Under Disturbance Compensation in a Current. 
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X 


u + 



V 


e 


= 0 


■K = 0 : • 

Rewriting these equations into matrix form, 


d 



A n A n 

5 

(x) 

■*1 


X 



= 






dt 



y4 2] A^ 




x 


where 

xj = IV r y x] , 
x 2 t = [v v c u c ) , 


0 10 0 
0 a n u 0 0 
10 0 0 
0 0 0 0 


(5.10) 


(5.11) 


57 






0 0 ol 


A 


12 


a 2l u 0 0 
1 1 0 
0 0 1 


A 


21 


0 a n u 0 0 
0 0 0 0 , 
0 0 0 0 


A 


22 


a n u 0 0 
0 0 0 , 
0 0 0 


B t T = [0, b 2 u\ 0, 0] , 
and 

B/ = [b x u\ 0, 0] . 

Equation (5.11) takes on the state space form of 


x = Ax + &S 


(5.12a) 


y - Cx 


(5.12b) 


where 


C - I , 


x x T arc the measurable states and xj are the states to be estimated or observed. 


Expanding (5.12a), 
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X, = -?Vi + A 12 X 2 + fi, 8 
x 2 = v4 21 Xj + + 5 2 5 . 

From the bucnberger reduced order observer development for x 2 , the estimated or 
observed states are 

x 2 = bx, + z, , (5.13) 


and 


z = Fz + Gx, + Hh , (5.14) 

where 


and 


>11 

II 

K> 

W 

-LA n , 

(5.15) 

G =/l 21 


(5.16) 


H = B 1 - LB l . (5.17) 

In the above equations, the L matrix needsto be determined. The MATRIX_x software 
package is unable to determine the L matrix directly because more than one output is 
measurable. Therefore, the L matrix will be determined manually. Let 



l U 



4< 

L = 

4, 

/« 


4, 


4, 

4z 

4, 

4. 
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and choose everything zero, except / 12 , / 23 and l v , so that 


From (5.15), 


L = 


0 / I2 0 0 
0 0 4j 0 

0 0 0 /„ 


F = A n - LA n 


^23 “4j ® 

0 0 -V 


(5.18) 


Now choose-the observer poles of s,, j 2 and s 3 to be at -1.0, -1.1 and -Irrespectively, 


which arc at least two times faster than the controller poles, defined in the above section, 
as required by a good observer design. Placing the observer poles in matrix form and 
equating to (5.18), 


= a n u ~ l n a n u t 


S 2 ~ ^23 


(5.19a) 

(5.19b) 


and 


^3 l v . 


(5.19c) 


Solving (5.19) for / J2 , and / ii . 
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, _ a ll u - s l 

ill ~ » 

a 2\ U 

(5.20a) 

^23 = “*2 

(5.20b) 

and 


/„ = -J 3 . (5.20c) 

From (5.20), the L matrix is now determined, therefore, F, G and H are also determined. 

From (5.14), the reduced order observer equations become 

z, = J I z 1 +(a 12 u-/ I2 a 22 u+J I / I2 )r+(6 I u 2 -/ |2 6,u 2 ) 8 

(5.21a) 

z 2 = s 2 z,+5 2 z 2 -/ 2J Msin(\j/-a)+J 2 / 23 y+5 2 / l2 r 

(5.21b) 

z 3 = s.z^s^jc . 

(5.21c) 


From (5.13), the equations for the estimated or observed quantities become 


v = z, + / 12 r , (5.22a) 

v e = z, + (5.22b) 

and 

u e = Zj + lyX . (5.22e) 

Due to the way h* was defined in (5.8), 
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(5.23) 


u Cohi = u c - ucosty - a) 


and 


" *e • (5.24) 

The sineand cosine terms are not linearized in (5.21b) and (5.23) to eliminate steady state 

errors in the observer when the angle y - a becomes significant at: steady state in the 
presence of strong currents. Since the:current perpendicular to the track and the current 
along thedine of each track will be different locally every time the AUV drives onto a 
new track, the current observer quantities of z 2 and' z 3 need to be reset every time a new 

way point is called for by the autopilot. The quantities z 2 and z 3 are used to determine 
i? and u. which determine v„ and u, . The equations used toTeset z,, z., v and 

c obs c obs Lie 

u c are 


and 


= v c cosa - « £ sina , 

(5.25) 

= t^xosa - v sina , 

-c c - - - - 

(5.26) 

Z 2 = V - 1 -l* > 

(5.27) 
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z 3 = ^ - l2 y • (5.28) 

These equations are used as the new way point is asked for and prior to entering the 


observer for the first time on the mew track. 


E. DISTURBANCE ESTIMATION AND COMPENSATION METHOD 

With the development of a current observer, reality can now be better incorporated 
into the controller design. The sliding surface (5.1) is now modified to 


r 


c = s l \^+s 2 v+s 3 r+s 4 y+ 






+s-. 


sin 


f. ^ 

v 

j 


(5.29) 


with 5 as in (3 ? 5). The rest of the development for the disturbance estimation arid 
•compensation method is exactly the same as for the disturbance compensation method. 
Figure 20 shows how well the disturbance estimation and compensation method works 
in the presence of the same environmental conditions as in Figure 17. Also the response 
of Figure 20 is virtually the same as for the disturbance compensation method in Figure 


17. Figure.20 used a k = 2.0, which is larger than k .. Again for ^ = 0.2, which is 


less than k cnl , unstable.behavior results, as seen in Figure 21. Figure 22 shows the results 

of the disturbance estimation and compensation method with no current. 

Results for multiple way points, using the disturbance estimation and compensation 
method, are presented tin Figures 23 and 24, with a target distance of 2 vehicle lengths. 
Figure 23 was conducted using the same current as in Figure 17. It can be seen that the 
disturbance estimatiomand compensation method works as well as the disturbance 
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Figure 20. Disturbance Estimation and Compensation, V c = 2, k K ; 
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Figure 23. Lane Changing Maneuver Under Disturbance Estimationsand 
Compensation in a Current. 
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compensation method, with perfect current input. Figure 24 shows how well the 
disturbance estimation and compensation method works for multiple way points with no 
current. In Figures 21 and 23 for zero estimated.current in the track, it can be seen that 
there is a small nonzero current in the graph of UCOOBS vs TIME. This small nonzero 
current is a result of the integratiomtime step not being small enough. Figures 20 through 
24 were all run at a 0.01 second time step. As the time step decreases, the time to run 
the program increased dramatically. 

F. MODIFIED DISTURBANCE ESTIMATION AND COMPENSATION 

In order to overcome the instability of the disturbance estimation and compensation 
method for k n < k eHl , a modified design will be considered. Similarly to the integral 

control method of Chapter IV, condition (4.12) will be required to be satisfied instead of 
(4.10), and the control law then becomes 


5 = (k l +fy{)y*(k 2 +fy^+(k 3 +&Jr.+%5y+k$atsgn(p) , 


(5.30) 


a = .s l y+s 2 y+'s 3 r-+sy+ 
Then, at steady state y t = 0 : provided 

k. > k' = 


—-- +S, 




sin' 


n ent 


) 


f \ 


( \ 
V 


v w / 


K U J 


(5.31) 


(5.32) 


(*, + ^,) sin' 1 

In this case, s can be found as the left eigenvector of the closed loop dynamics matrix 
that corresponds to the eigenvalue For small Values of the same s and k can be 
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used as before. If k K < k cri {, this modified design develops a finitej steadv state error, y s . 


computed from 8, = 0 and o s >4- Results are presented in Figure 25 for the same 

conditions as in Figure 21, with £.= 0.5. It can be seen that the nonzero value of q 
stabilized the vehicle and reduced the path error. This, however, is not always the case. 
The explanation lies in the fact that nonzero values of E, raise the critical -k as shown 

in (5.32). The steady state cross track errors versus £ and k n are shown in.perspective 

views in Figures 26, 27 and 28, for u = 6 ft/sec and v e = 1,2.5 and 4 ft/sec, respectively. 

It can be seep that for k H < k criI , y t is reduced by increasing , although beyond a 

certain point the corresponding reduction in y i is insignificant. For k n > /: enl ,. the value 

of ^ should not be increased beyond the value that renders k n = k c J in (5.32), unless the 

vehicle is expected to operate in high current environments that would increase the value 
of k crit in (3.14). 

Chapters 3, 4 and 5 have dealt with methods to control the cross track error, due 
to a constant disturbance. to zero. Now that the methods have been developed and shown 
to work well, the next chapter will devise a technique to help optimize the time to turn, 
so that the turn is initiated and conducted in the most efficient manner given any 
environmental conditions. This technique will be referred to as the:leading track control 
monitoring technique. 
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VI. LEADING TRACK CONTROL MONITORING TECHNIQUE 

A. INTRODUCTION 

Methods for controlling a cross track error, due to a constant disturbance, have been 
developed in Chapters 3,4 and 5. In each method, it was seen; that thcdesired track was 
attained within the limits of the theory, with the appropriate values of k r > k cril . It was 

also seen that, for multiple way points, the target distance played a major role on how 
well the AUV initiated the turn so as to attain the next track. The ability of the AUV to 
turn depends on the environmental conditions, the vehicle response characteristics and the 
turning angle, as discussed in Chapter III. This chapter will develop a technique that 
monitors the leading track, in order to determine the correct time for the AUV to initiate 
the turn with no overshoot and minimal rudder use. This technique is referred to as 
leading track control monitoring. 

The concept of leading track control monitoring is to use two legs, the current leg 
to control the cross track error, or track deviation; and the second leg, to control course 
deviation, or to determine the correct time to initiate,the turn onto the new track [Ref. 
16]. This chapter will also show results on how weh the leading track control monitoring 
technique works as compared to using the previous control methods; i.c., disturbance 
compensation, disturbance estimation and compensation and integral control; with various 
target distances. 
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B. LEADING TRACK CONTROL MONITORING DEVELOPMENT 

A simple technique to automatically initiate the turn onto the new track at the 
proper time will be used for the course change from the current track to the leading-track. 
The vehicle is assumed originally to be sailing on the current track. Now consider the 
application of the control law for the leading track, simultaneously, which will order the 
rudder command to drive the vehicle onto that track. Two control laws are constructed: 
one control law for the current track, which is used to reduce the track deviation; and one 
control law for the leading track, which is used to monitor course deviation. To make 
a smooth connection from the current track to the leading track, the control law for the 
leading track will be monitoredin addition to the present control law for the current track, 
simultaneously. In the beginning, the track deviation is much larger than the course 
deviation. However, in the mean time, the track deviation will be decreased and the 
course deviation will become dominant. Therefore, a smooth connection can be attained 
by switching the actual control from the current track to the leading track as soon as the 
monitored leading track control reaches zero. The leading track control that reaches zero 
is the point the rudder for the leading track changes sign from positive to negative or vice 
versa. 

C. RESULTS 

Figures 29 through 34 show the results of the leading track control monitoring 
technique, which automatically determines the point to initiate the turn onto the next 
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Figure 29. Leading Track Monitoring Technique: 5° Course Change. 
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Figure 30. 5° Course Change: Fixed Target Distances of 0.5, 2 and 4 Vehicle 
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Figure 32. 45° Course Change: Fixed Target Distances of 0.5, 2 and 4 Vehicle 
Lengths. 
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track, applied to the disturbance estimation and compensation method and compared to 
fixed target distances of 0.5,2 and 4 vehicle lengths, respectively, forno current. Figures 
29, 31 and 33 show the results of the leading track-control monitoring.technique, with the 
disturbance estimation and compensation method, for course changes of 5°, 45° and 90°, 
respectively. The leading track monitored rudder angle and course deviation are referred 
to as DR99 and YCTE99 in the graphs, respectively. The leading track control 
monitoring technique can also be used with the other control methods discussed in the 
previous chapters, however, for Figures 29 through 34, the disturbance estimation and 
compensation method is utilized. Figures 30, 32 and 34 show the results of the 
disturbance estimation and compensation method for target distances of 0.5, 2 and 4 
vehicle lengths, from top to bottom respectively. For each course change of 5°, 45° and 
90°, there is one target distance that is best for that course change, and it will not 
necessarily be the best for the other course changes. For example, Figure 30 shows that 
a target distance of 2 vehicle lengths is best for a course change of 5°. However, for the 
course changes of 45° and 90°, target distances of 2 to 3 and 4 vehicle lengths, 
respectively, are best for these course changes. The leading track control monitoring 
technique eliminates the need to worry about what target distance is required because the 
technique automatically determines the distance.required to initiate the turn onto the next 
track without any overshoot and minimal rudderuse. 

Figures 35 through 39 show the performance of the leading track monitoring control 
technique in the presence of a current for the multiple way points used in the previous 


83 







DR99 YLCRSE (SHIF LENGTHS> Y (SHIP LENGTHS) 




0.0 30.0 60.0 90.0 120.0 0.0 30.0 60.0 90.0 120.0 

TIME lSEC) TIME iSECi 


Figure 35. Lane Changing Maneuver, Disturbance Estimation and Compensation, 
Leading Track Monitoring Technique: Current U c = 0, V c = 2, 
Course Change 5 : 10. 
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Figure 37. Same As Figure 35 with Course Change 5 : 5. 
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Figure 39. Lane Changing Maneuver, Disturbance Estimation and Compensation, 
Leading Track Monitoring Technique: CuiTent U e - V c - -2.5, 
Course Change 5 : 10. 
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chapters. Figures 35 through 37 were conducted at U c = 0.0 and V e = 2.0 ft/sec for 

increasing course changes. It can be seen that the leading track control monitoring 
technique works superbly in the presence of a current for increasing course changes with 
no overshoot for each turn onto the leading track, with minimal use of the rudder. 
Figures 38 and 39 show the leading track control monitoring technique for currents other 
than that used in Figures 35 through 37. Figure 38 used a current of U c - V c - 2.0 

ft/sec and Figure 39 used a current of U c = V c = -2.5 ft/sec. Both of these figures 

reveal that the leading track control monitoring technique can handle very large currents, 
within the physical constaints of the vehicle, very well. 

The automatically selected target distance d : (in vehicle lengths), by this technique, 
is plotted in Figure 41 for different current magnitudes and directions (the symbols are 
explained in Figure 40) and u = 6 ft/sec. It can be seen that d depends on both the 
strength v and orientation 8 of the current and the turning angle a. As the angle a is 
increased, d is also increased, as expected. The same is true for increasing current speed. 
For very small changes in vehicle path a, the leading track control monitoring technique 
tends to be conservative; i.e., it initiates the turn early with very little rudder usage. If 
the technique is modified such that the actual switching occurs when the monitored rudder 
angle reaches aspecificd value (such as its saturation limit) after the zero crossing, then 
smaller target distances can be achieved for small a. Also, the technique in its current 
application, cannot handle turning angles more than 90°. Although such turns arc rarely 
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LEGEND 


d target distance (automatically selected) 

a course change (commanded turning angle) 

0 current direction 

v current resultant speed 


Figure 40. Nomenclature for Leading Track Monitoring Technique. 





Figure 41. Automatically. Selected Target Distances for Different Current 
Magnitudes and Directions. 











demanded for by the path planner, the technique can; be modified to allow for these 
drastic turns if desired. 

In the final chapter, noise will be introduced into the measureable parameters; i.e., 
y, r, y or x; and the effects will be evaluated. Also, guidelines will be developed for 
suppressing the effects of measurement noise. 
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VII. ROBUSTNESS TESTS AND SENSOR NOISE EFFECTS 

A. INTRODUCTION 

The characteristics of the controllers that were designed in the previous chapters are 
.analyzed here with a view to their robustness properties with respect to unmodelled 
dynamics and actual/mathematical model mismatch. For the sake of brevity, emphasis 
is placed on the disturbance estimation and compensation design. The effects of sensor 
noise arid sensor drift are also evaluated through, a series of digital simulations. This 
brings another level of realism into the design. 

B. ROBUSTNESS PROPERTIES 

The effect* of the sway velocity observer is evaluated in Figure 42. Curve \ is 
obtained by using the observed value of the sway velocity v, whereas, Curve 2 is obtained 
by assuming that v = 0. The vehicle speed u wasdcept constant at u = 6 ft/sec, and the 
‘lateral current was v c = 2 ft/sec. Disturbance estimation and compensation was used 
with k K = 5 andrtj) = 0.5. It can be seen that the response of the two curves is almost 

identical; It can be, therefore, concluded that the sway velocity does not appear to-be 
very significant for track control design. This result is analogous to the Line of Sight 
navigation case [Ref. 12]; 

Results for different forward speeds are shown in Figure 43, for the same conditions 
as in the previous test. Curve 2 was obtained for u = 6 ft/sec (nominal design), while 
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Figure 42. Robustness Test: Sway Velocity Effect. 
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Figure 43. Robustness Test: Forward Speed Effect. 
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Curve 1 is for u = 3 ft/sec and: Curve 3 is for u = 12: ft/sec, with the same gains and; 
sliding plane coefficients as for the nominal case. It can be seen that large deviations in 
the forward speed can be accomodated by the controller without the need for gain 
scheduling. 

The robustness oflthe compensator is also evaluated in Figure 44 for a drastically 
off design case (Curve 2), which is shown along with the response of the nominal design 
(Curve 1). The same current v. = 2 ft/sec is present. For Curve 2, the values of the 

hydrodynamic coefficients Y v and N r were reduced in the equations of motion to half of 

their actual values, and the rudder coefficients and N & were increased to twice their 

actual values. Both of these changes correspond to a more responsive and less damped 
vehicle; The controller and observer were designed for the true values of the coefficients, 
so that the vehicle is operating with large errors in the knowledge of its dynamics. The 
results of Figure 44 demonstrate the ability of the controller to meet its mission 
requirements even under unrealistic errors in the design. The track overshoot for the off 
design case is attributed to the slower convergence of the current observer to the true 
current speed, as is also shown in Figure 44. 
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C. EFFECTS OF SENSOR NOISE 

So far, incomplete but perfect state measurement has been assumed. To analyzeuhe 
effects of sensor noise on the sliding mode track controller with the disturbance 
estimation and compensation method,;the following scenario is considered. The vehicle 
is moving with u = 6 ft/sec in a lateral current v c = 2 ft/sec. Controller poles are 

selected at -0.25 and observer poles at -0.50. The measurable quantities are \{/, r and y, 
and the noise is simulated by Gaussian distribution with typical standard deviations of 0.1 
degrees for \|T, 0.01 degrees/sec for r and 0.1 ft for y. All simulations are performed 
using Euler integrations with time step At = 0.1 seconds. This corresponds to a sample 
rate of 10 hertz, which is reasonable. All results show time histories of the exact, not the 
measured, lateral deviation y, in vehicle lengths, and the actual rudder angle 5 in degrees. 
The same scale has been kept for all graphsTor comparison. 

The results of the simulation for k n -2 , <■> = 0.5 and for noise free sensors are 

presented in Figure 45. When the assumed noise is introduced, as in Figure 46, the actual 
y does not differ significantly. The rudder angle 5, however, is chattering so that th*' 
design cannot be accepted. If the value of 4> is increased to 5, then the level of rudder 
chattering is significantly reduced at the expense of a slower vehicle response, as shown 
in Figure 47. The level of <j> is ultimately related to the standard deviation of the sliding 
plane a. If a faster vehicle response is needed, then <j> can bccumc a function of g, so 
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ithat it is small away from the sliding plane and becomes larger as the system approaches 
a = 0, Keeping the same if = 0.5 and reducing k M to 0.5 helps to reduce the level of 

chattering, as shown in Figure 48. This, however, has the effect of possibly sacrificing 
stability or steady state accuracy, as analyzed in the previous chapters. I: follows then, 
that the first action to suppress the noise effects must be to increase the value of <> . 

Another way to further improve on the response, in a noisy set of measurements, 
is to introduce a first order lag between commanded and actual rudder angle. If T r 

denotes the artificial (software) steering gear time?constant, then 

7*$ + 8 = 8 c , (7.1) 

where 5 c is the commanded rudder angle and 5 the actual rudder angle. A time constant 

T r = 0.5 seconds, which is five times higher than the integration step, should provide 
enough noise attenuation, since the comer frequency of (7.1) is 2, while the frequency of 
the noise is 10. At the same time a value of 7* r = 0.5 seconds is small enough so that 

the transient response characteristics of the vehicle are not significantly affected. The 
results are shown in Figure 49, and for comparison, the response of the identical system, 
with noise free sensors, is shown in Figure 50. If a faster response is necessary, then the 
controller has to be redesigned by taking (7.1) as an extra state equation. 

Very low values of T r (of the same order of magnitude as a r) do not have any 

visible effects in noise reduction, while large values of T r can deteriorate the transient 
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response characteristics significantly. The latter is demonstrated in Figure 51 for 7=5 
seconds. Finally, even if 4 is kept at 0.5, introduction of T r = 0.5 seconds reduces the 

chattering significantly, as seen in Figure 52. 

It follows that increasing the value of <j> and introducing an appropriate software 
rudder time constant T are two major guidelines for reducing the effects of sensor noise 

and still keep satisfactory transient response. Of course, observer gains can be established; 
be a Kalman filter design. This should help in minimizing the variance of the control 
effort and response even further. 

D. EFFECTS OF SENSOR DRIFT 

Having analyzed the effects of sensor noise, a different aspect of sensor 
imperfection, namely sensor drift, will now be investigated. The most critical sensor drift 
for the track keeping problem is the offset or positional drift of the Inertial Navigation 
System. Along with the simulated'noise of.the previous section, the offset measurement 
is assumed to experience a drift of one vehicle length in ten dimensionless seconds before 
the next exact navigational update comes up. For simulation purposes the drift is 
assumed to be linear between the two updates. Results for the lateral offset y and rudder 
.angle 5 are presented in Figure 53 for u = 6 ft/sec, k n = 2, <}> = 5, T r = 0.5 seconds 

and lateral current v c = 2 ft/sec. As expected, the vehicle drifts off the y = 0 track 
following the sensor read but. 
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This lateral offset-drift, or bias,/cannot unfortunately be estimated by an observer 
because the dynamic system is unobservable unless y is measured; One way to improve 
the response is to record the two most recenr navigational updates together with the 
corresponding sensor readings. Then the v-r,y. drift is assumed to be linear, and this 
result is extrapolated until the next navigation.? fix, and the process is repeated. The 
vehicle response is now satisfactory, as shown n Figure 54, unless the actual sensor drift 
is significantly different:than the extrapolated, as is tl,e case between 10 and 20, and 
30 and 40 dimensionless seconds. 

E. NAVIGATIONAL UPDATES EFFECT 

So far, knowledge of y is assumed to occur at the same raiems the simulation step, 
or the autopilot updates in y and r. In reality, this will probably not be the case, since 
measurement of y is more involved than y or r, and will thus occur at a slower fate. The 
effects of updating the cross track error at a slower rate are analyzed in Figures 55 
through 61 for u = 6 ft/sec and v c = 2 ft/sec. The actual path, not the one that is 

available to the compensator at all times, is plotted versus time using the same scale in 
all figures for comparison. The response of the nominal design is showfrin Figure 55 fori. = 5 

and $ = 0.5. The response, when the cross track error y updates are 10,20 and 30 times 
slower than the integration step, is presented in Figures 56 through 58. It can be seen 
that in the latter case, the vehicle is unstable. It should be pointed out, though, that it 
should not really be expected to apply a cross track error compensator in a strong current 



















o 



l 


Figure 55. Navigational Updates Effect: Nominal Design. 
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Figure 56. Navigational Updates Effect: Navigational Updates 10 Times Slower 
Than Autopilot 
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Figure 57. Navigational Updates Effect: Navigational Updates 20 Times Slower 
Than Autopilot. 



Figure 58. Navigational Updates Effect: Navigational Updates 30 Times Slower 
Than Autopilot 
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Figure 59. Navigational Updates Effect: Same As Figure 58 with Assumed Linear 
Variation Between Consecutive Navigational Updates. 
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Figure 60. Navigational Updates Jffect: Same As Figure 58 with $ = 5. 
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environment when the cross track error is available only once every thirty sample 
instances! 

Aniimprovement can be achieved if the actual-track deviation isassumcd to vary 
linearly between two consecutive navigational updates. The results arc presented in 
Figure 59 where the improvement over Figure 58 is evident. The response appears now 
to be bounded about the y = 0 track. Further improvement can be achieved, if more than 
two navigational updates arc kept and a spline curve is fit among them. 

A final improvement is possible if the value of $ is increased. Such an increase 
was found to be advisable for noise suppression as well. Results for 6 = 5 arc presented 
in Figure 60 for a navigational update factor of 30. Unlike the case 6 = 0.5 of Figure 
58, the response is now stable. When the linear extrapolation technique of the previous 
paragraph is combined with the above increase in 6, the response is faster and less 
oscillatory, as depicted in Figure 61. Introduction of the software steering gear lag. 
T r = 0.5 seconds, docs not alter significantly the response, as shown in Figure 62. 
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CONCLUSIONS AND RECOMMENDATIONS 

A. CONCLUSIONS 

A methodology for designing sliding mode autopilots for vehicle maneuvering and 
track following control has been presented. The methods are suitable for a wide variety 
of related control problems. Also, a technique has been presented to drive the vehicle 
onto the next track, with no-overshoot and minimal rudder use, and which can be used 
with any of the control methods presented. Finally, noise in the measurable parameters 
Was evaluated and guidelines for suppressing the effects of this noise were presented. In 
the present case of the AUV track keeping, the principal conclusions of this work can be 
summarized in the following paragraphs and in Figures-63 and 64. 

As seen in Figure 63, it is shown that the cross track error control provides better 
track keeping characteristics than heading (Line of Sight) control. The premise of this 
research was the necessity for accurate track keeping of autonomous underwater vehicles 
for autonomous navigation of a vehicle through confined spaces, and in the presence of 
obstacles and cross current environments. Thus, it is paramount that AUV’s have the 
ability to follow a track, with minimal cross track error, using the control methods 
developed in this research. The Line of Sight scheme is very efficient and provides 
smooth turning characteristics during rapid maneuvering and course changing. For 
transits along straight line tracks, however, the stability of the scheme is not guaranteed 
for every way point, target distance combination. This is demonstrated in Figure 63 
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where the initial heading is 30°. Curve 1 corresponds to the response of the cross track 
error design. The rest of the curves correspond to stable responses of Line of Sight 
designs. Curve 2 corresponds to five way points and target distance d = 2 vehicle 
lengths, Curve 3 to three way points and d = 0.1 vehicle lengths and Curve 4 to two way 
points and d = 2 vehicle lengths. As can be seen, Curve 1 is superior to all. 

Analytical evaluation of the stability, criterion, in the presence of a constant 
disturbance, was achieved. The cross track error controller developed ^steady state track 
error for this case, 

For the integral control method, it was; shown that for asnonlinear gain, k n , greater 

than or equal to the theoretical critical gain, k etit , there was zero steady state error. 

However, when the nonlinear gain was less than the-theoretical critical gain, there was 
a finite, but stable steady state error. When the integral control method was modified, 
a zero steady state error was seen for a nonlinear gain greater than or equal to the 
theoretical critical gain, as Tor the integral control method. However, when the nonlinear 
gain is less than the theoretical critical gain, a zero steady state error results vice the 
finite, stable steady state error, as in the integral control method. Due to the general 
oscillatory response of the integraLcontrol method, it is best to keep the integrator pole 
closer to the origin. When utilizing the integral control method, it would: be best 
employed by switching the integrator off, if the oscillatory response is too much during 
transients, and by switching the integrator on for long, straight transit tracks. 
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In the disturbance estimation and compensation method, a zero steady state error 
resulted when the nonlinear gain was greater than Or equal to the theoretical critical gain. 
An unstable:response resulted for this control method when the nonlinear gain was less 
than the theoretical critical gain. For the modified 1 disturbance estimation and 
compensation method, a zero steady state error again resulted for the nonlinear gain, 
greater than or equal to the theoretical critical gain. Now for the nonlinear gain less than 
the theoretical critical gain, a stable nonzero steady state error resulted. In general, the 
response is not oscillatory for the disturbance estimation and compensation method, but 
this method only controls the cross track error, due to a constant disturbance, when the 
constant disturbance is a current. On the other hand, the integral control method can 
control the cross track error, due to any constant disturbance, not only a current. These 
results are seen in Figure 64. The vehicle was subjected to a sway force disturbance 
equivalent to a 1 ft/sec current and a yaw moment disturbance equivalent to a 2 ft/sec 
current, thus the constant disturbances do not correspond to any physically realizable 
currents. The integral control method (Curve 1) brings the vehicle onto the desired track, 
whereas, the disturbance estimation and compensation method (Curve 2) and the plain 
cross track error designs (Curve 3) both experience nonzero steady state errors. Of 
course, if the disturbance observer is modified to take into account a general sway force 
and yaw moment, then the response would experience zero steady state error as the 
integral control method does. 

The leading track control technique was seen to improve the-turning characteristics 
of the vehicle, so as not to overshoot the next track and to use minimal rudder. The 
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distance from the way point to accomplish this was determined automatically by the 
technique. This technique can be used with all the control methods developed: in this 
research. 

The cross track error controller proved to be very robust and was able to handle a 
wide range of parameter variations without loss of stability. 

The effects of sensor noise and sensor drift Were numerically evaluated. With 
appropriate modifications in the control law,;it was shown: that sensor noise and sensor 
drift could; be minimized. Finally, it was demonstrated that positional updates are very 
important for accurate track keeping, but they can occur at a. slower rate than the rest of 
the autopilot updates. 

B. RECOMMENDATIONS 

Some recommendations for further research are as follows: 

• Experimental verification using the full scale NPS AUV II after its hydrodynamic 
coefficients have been reliably established. 

• Incorporation of Kalman filter designs to further improve the response and to 
reduce the effects of sensor noise and random disturbances. 

• Simulation of an Inertial Navigation System required for positional updates. 
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APPENDIX A 


C ADDED CURRENT AS A DISTURBANCE IN THE CONTROL LAW 
C ADDED CURRENT OBSERVER 

C ADDED XI TO MODIFY THE DISTURBANCE AND ESTIMATION COMPENSATION 
C METHOD 
C 

REAL MASS,LATYAW,NORPIT 

REAL MM(6,6),G4(4),GK4(4), BR(9),HH(9 ) 

REAL B(6,6),BB(6,6) 

REAL A{12,12), AA(12,12),INDX(100) 

REAL XPP , XQQ , XRR ,XPR 
REAL XUDOT ,XWQ ,XVP ,XVR 
■ REAL XQDS ,XQDB ,XRDR ,XW 
REAL XWW ,XVDR ,XWDS ,XWDB 
REAL XDSDS,XDBDB ,XDRDR ,XQDSN 
REAL XWDSN ,XDSDSN 

REAL TIME'S,EITA,UBAR,UHAT,COMZ/BAR, SIM,DE,SAT, VHAT, ZZOBS, ZZOBSDOT, SIM1 
REAL SSI,SS2,UD,XD, YD, TD, TNWP, XA, YA, HD, HDMDEG, DAWAY, SATSGN1, SATSGN2 
REAL NAVUPDATE,TNAV,-TARGET, FF, GG,HHH, LLL, HDP, HDM, VCUR,UCUR,UCO, VCO,WCO 
REAL UCOOBS,VCOOBS,VCOHAT,UCHAT 
INTEGER DV 
C 

C LATERAL HYDRODYNAMIC COEFFICIENTS 

C 

REAL YPDOT ,YRDOT,YP0 ,YQR 
REAL YVDOT ,YP ,YR ,YVQ 
REAL YWP ,YWR ,YV ,YVW 
REAL YDR ,CDY 
C 

C NORMAL HYDRODYNAMIC -COEFFICIENTS 

C 

REAL 2QDOT ,2PP,ZPR ,,2RR 
REAL ZWDOT ,2Q ,ZVP ,ZVR 
REAL ZW- ,2W , ZDS ,ZDB 

REAL ZON ,ZWN ,ZDSN- ,CDZ 
REAL ZHADOT,ZHAT 
C 

C ROLL HYDRODYNAMIC COEFFICIENTS 

C 

REAL KPDOT ,KRDOT ,KPQ , KQR 
REAL KVDOT , KP ,KR ,KVQ 
REAL KWP , KWR ,KV ,KVW 

REAL KPN , KDB 
C 

C PITCH HYDRODYNAMIC COEFFICIENTS 

C 

REAL MQDOT ,MPP ,HPR,MRR 
REAL MWDOT , MQ ,MVP ,MVR 
REAL MW , MW ,MDS ,MDB 
REAL MQN , MWN ,MDSN 
REAL QHADOT,QHAT,THADOT,THAT 
C 

C YAW HYDRODYNAMIC COEFFICIENTS 

C 

REAL NPDOT,NRDOT,NPQ ,NQR 
REAL NVDOT , NP , ! NR ,NVQ 
REAL NWP , NWR ,NV ,NVW 
REAL NDR 
C 

C MASS CHARACTERISTICS OF THE FLOODED VEHICLE 

C 
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REAL WEIGHT , BOY >VOL ,XG 

REAL YG , ZG ,XB ,ZB 

REAL IX , IY ,IZ ,IXZ 

REAL IYZ , IXY ,YB 

REAL L , RHO ,G ,NU 

REAL AO ,KPROP ,NPROP , XlTEST 

REAL DEGRUD ,DEGSTN 

COMMON /BLOCKl/ F(12), FP(6), XMMINV(5,6), UCF 

INTEGER N, IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 

REAL VECV1 (9) , VECV2 (9 ) , X{12),VECH1 ( 9 ),VECH2 ( 9 ) , XI (9) 

RUDDER COEFFICIENTS 

PARAMETER ( DSMAX- -0.175) 

LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(XPP - 7.E-3 ,XQQ - -1.5E-2 ,XRR - 4.E-3 ,XPR -7.5E-4, 

& XUDOT—7.6E-3 ,XWQ - -2.E-1 ,XVP - -3.E-3 ,XVR - 2.E-2, 

& XQDS-2.5E-2 ,XQDB—2.6E-3: ,XRDR- -l.E-3 ,XW -5.3E-2, 

& XWW -1.7E-1 ,XVDR«1.7E-3 .XWDS-4.6E-2 ,XWDB« l.E-2, 

& XDSDS- -l.E-2 ,XDBDB- -8.E-3 ,XDRDR- -l.E-2 ,XQDSN« 2.E-3, 

S XWDSN-3.5E#3 ,XDSDSN- -1.6E-3 ) 

LATERAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(YPDOT-1.2E-4 ,YRDOT«l.2E-3 ,YPQ - 4.E-3 ,YQR —6.5E-3, 

5 YVDOT—5.5E-2 ,YP - 3.E-3 ,YR - 3.E-2 ,YVQ -2.4E-2, 

6 YWP -2.3E-1 ,YWR —1.9E-2 , YV - -l.E-1 ,YVW -6.8E-2, 

& YDR -2.7E-2 ,CDY -3.5E-1) 

NORMAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(ZQDOT—6.8E-3 ,ZPPil.3E-4 ,ZPR-6.7E-3 ,ZRR—7.4E-3, 
& ZWDOT—2.4E-1 , ZQ »-rl. 4E-1 ,ZVP i-4.8E-2 ,ZVR -4.5E-2, 

5 ZW - -3.E-1 ,ZW «-6.8E-2’ ,ZDS —7.3E-2 ,ZDB —2.6E-2, 

6 ZQN —2.9E-3 ,ZWN —5.1E-3 ,ZDSN!- -l.E-2 ,CDZ - 1.0) 

ROLL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(KPDOT- -l.E-3 ,KRDOT—3.4E-5 ,KPQ —6.9E-5 ,KQR -1.7E-2, 

5 KVDOT-1.3E-4 , KP — 1.1E-2 ,KR»-8.4E-4 ,KVQ—5.1E-3, 

6 KWP —1.3E-4 , KWR -1.4E-2 ,KV -3.1E-3 ,KVW —1.9E-1, 

& KPN —5.7E-4 , KDB-- 0.0 ) 

PITCH HYDRODYNAMIC COEFFICIENTS 

PARAMETER(MQDOT—1.7E-2 ,MPP -5.3E-5 ,MPR - 5.E-3 ,MRR —2.9E-3, 

5 MWDOT—6.8E-3 , MQ---6.8E-2 ,MVP-1.2E-3 ,MVR-1.7E-2, 

6 MW - l.E-1 , MW —2.6E-2 ,MDS —4.1E-2 ,MDB -6.9E-3, 

& MQN —1.6E-3 , MWN —2.9E-3 ,MDSN —5.2E-3) 

YAW HYDRODYNAMIC COEFFICIENTS 

PARAMETER (NPDOT— 3.4E-5 ,NRDOT»-3.4E-3',NPQ —2.1E-2 , NQR -2.7E-3; 

& NVDOT-1.2E-3 , NP —8.4E-4 ,NR —1.6E-2 ,NVQ - -l.E-2, 

& NWP —1.7E-2 , NWR -7.4E-3 ,NV ->7.4E-3 ,NVW —2.7E-2, 

& NDR —1.3E-2) 

MASS CHARACTERISTICS OF THE FLOODED VEHICLE 
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PARAMETER( WEIGHT -12000. BOY -12000. ,V0L -200. ,XG - 0. 


& YG - 0.0 

, ZG - 0.20 

,XB - 0. 

,ZB — 0.0 , 

& IX - 1500. 

, IY - 10000. 

,IZ - 10000. 

,IXZ - -10. , 

& IYZ - -10. 

, IXY - -10. 

,YB - 0.0 , 


( L -17.4 

, RHO - 1.94 

,G - 32.2 

,NU - 8.47E-4 

t A0 - 2.0 

,KPROP - U. 

,NPROP - 0. , 

XlTEST- 0.1 , 


i DEGRUD- 0.0 ,DEGSTN- 0.0) 

INPUT INITIAL CONDITIONS HERE IT REQUIRED 

OPEN(20,FILE-'MODEL!.DATSTATUS-'NEW' ) 

NUMPTS-0.0 
DV-I.O 

*********************objain INITIAL INFORMATION***************** 

OPEN (30,FILE-'INITIAL.DAT'.STATUS-'OLD') 

READ (30,*) UO.RPM 

READ (30,*) UD,NAVUPDATE,SIMl,DELT 

READ (30,*) XD2,YD2,COM2 

. . , READ IN STEERING AND SLIDING SURFACE GAINS,INITIAL CURRENTS, 
. ... AND SATURATION DESIRED 

OPEN(21,FILE-'SMCINT.DAT',STATUS-'OLD') 

READ(21,*) GG1,GG2,GG3,GG4,GG5 
READ(21,*) SP1,SP2,SP3,SP4,SP5 
READ(21,*) UCO,VCO,WCO 
READ(-21, *) AKN,SSPHI 
READ(21,*) IPT5,XI 

UCOOBS-O.0 
VCOOBS-O.0 
V0 - 0.0 
W0 - 0.0 
P0 - 0.0 
Q0 - 0.0 
R0 - 0.0 
PHI0 - 0.0 
THETAO - 0.0 
PSI0 - 0.0 
XPOSO-O.O 
YPOSO-O.0 
ZPOSO-O.O 
XD1-0.0 
YD1-0.0 
DB- 0.0 
DS - 0.0 
DR - 0.0 
LATYAW - 0.0- 
NORPIT - 0.0 
RE - U0*L/NU 
TNAV-0 
XA-XPOSO 
YA-YPOS0 

TIME-0.0 
TIMEO-O.0 
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u - uo 

V - VO 

w - wo 

P - PO 
Q - QO 
R - RO 

XPOS - XPOSO 
YPOS - YPOSO 
ZPOS - ZPOSO 
PSI - PHIO 
THETA - THETAO 
PHI - PHIO 
QHADOT-O.0 
THADOT-O.0 
ZHADOT-0.0 
QHAT-0.0 
THAT-0.0 
ZHAT-0.0 
VHAT-0.0 
ZOBSDOT-O.0 
ZZOBS *0.0 

;.. DEFINE THE LENGTH X AND HEIGHT HH TERMS FOR THE DRAG INTEGRATION 


XI (1) 

m 

-105.9/12. 

XI (2) 


-99.3/12. 

Xl ( 3) 

m 

-87.3/12. 

Xl(4) 

m 

-66.3/12. 

XI ( 5) 

m 

72.7/12. 

Xl(6) 

• 

83.2/12. 

XI ( 7) 

m 

91.2/12. 

XI (8) 

m 

99.2/12. 

XI (-9) 

m 

103.2/12. 

HH(-l) 

m 

0.00/12. 

HH{ 2) 


8.24/12. 

HH (3) 

m 

19.76/12. 

HH( 4 ) 

m 

29.36/12. 

HH( 5) 

m 

31.85/12. 

HH( 6) 

m 

27.84/12. 

HH{7) 

m 

21.44/12. 

HH (8 ) 

m 

12.00/12. 

HH( 9) 

m 

0.00/12. 

BR(1) 

m 

0.00/12.0 

BR(2) 


8.24/12.0 

BR(3) 

m 

19.76/12.0 

BR( 4) 

m 

29.36/12.0 

BR( 5) 

m 

31.85/12.0 

BR(6) 

u 

27.84/12.0 

BR ( 7) 

m 

21.44/12.0 

BR( 8) 

m 

12.00/12.0 

13R(9) 

m 

0.00/12.0 

MASS - WEIGHT/G 

N - 6 

DO 15 

j 

- 1, N 

DO 

1C 

K - 1,N 


XMMINV(J,K) - 0.0 
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MM(J,K) - 0.0 

10 CONTINUE 
15 CONTINUE 

C 

DEFINE HASS MATRIX 

MM(1,1) - MASS -((RHO/2)*(L**3)*XUD0T) 

MM{1,5) - MASS*ZG 
MM(1,6) - -HASS*YG 

MM(2,2) - MASS -({RHO/2)*(L**3)*YVDOT) 

MM{2,4) - -MASS*ZG -((RHO/2)*(L**4)*YPDOT) 
MM(2,6) - MASS*XG - ((RHO/2)*(L**4)*YRDOT) 

MM(3,3) » MASS - ((RHO/2)*(L**3)*ZWDOT) 

MM(3,4) - MASS*YG 

MM(3,5) « -MASS*XG -((RHO/2)*(L**4)*ZQDOT) 

MM( 4,2) - -MASS*ZG - (=( RHO/2)*(L**4 ) *KVDOT) 

MM(4,3) - MASS*YG 

MM(4,4) - IX - ((RHO/2)*(L**5)*KPDOT) 

MM(4,5) - -IXY 

MM(4,6) - -IXZ -((RHO/2)*(L**5)*KRDOT) 

MM(5,1) - MASS*ZG 

MH{ : 5,3) - -MASS*XG -( (RHO/2)*(L**4)*MMDOT) 

MM(5,4) - -IXY 

MM(5,5) - IY -((RHO/2)*(L**5)*MQDOT) 

MM(5,6) - -IYZ 

MM(6,1) - -MASS*YG 

MM(6,2) - MASS*XG -((RHO/2)*{L**4)*NVDOT) 

MM(6,4) - -IXZ - {(RHO/2)*(L**5)*NPDOT) 

MM(6,5) - -IYZ 

MM( 6,6 ) - IZ - u( RHO/2) * (L**5 ).*NRDOT)' 

OBSERVER POLES 

51— 1.0 

52— 1.1 

53— 1.2 

OBSERVER A MATRIX CONSTANTS AND B MATRIX CONSTANTS 

All—0.04530 
A12—0.35119 
A21—0.002795 
A22—0.09568 
B1 - 0.011432 
B2 —0.004273 

*****ROUTINE FOR INVERTING THE MM MATRIX***** 

DO 12 I-1,N 
DO 11 J-1,N 

XMMiNV(I,J)-0.0 

11 CONTINUE 
XMMINV(I,I)“1 

12 CONTINUE 

CAUL INVTA(MM,N,INDX,D) 
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DO 13 Jil,N 

CALL INVTB(MM,N,INDX,XMMINV(1,J)) 

3 CONTINUE 

************ INPUTS ************* 

RUDDER AND DIVE PLANE COMMANDS 

SIM-SIM1/DELT 
TIME-0.0 
DS- 0.0 
DR- 0.0 
DB- 0.0 
EITA-4.0 
BAR-.4 

YINTGR-0.0 
SSPHM—SSPHI 

.. . DETERMINE THE ANGLE ALPHA;XPOS AND YPOS ARE GLOBAL COORDINATES 

XPOSl-XDl 

YPOSl-YDl 

XPOS2-XD2 

YPOS2-YD2 

CALL ANGLE!XFOS1,YPOS1,XPOS2,YPOS2,ALPH) 

... DETERMINE THE LENGTH OF INITIAL PATH 

XT-SQRT!(XPOS2-XFOS1)**2 + (YPOS2-YPOS1)**2) 

XT-XT*L 

*******************SXMULATION BEGINS **************** 

DO 100 I-l.SIM 
PROPULSION MODEL 
SIGNU - 1.0 

IF (U.LT.0.0) SIGNU - -1.0 
IF (ABS(U).LT.X1TEST) U - XlTEST 
SIGNN - 1^,0 

IF (RPM.LT. 0.0 ) SIGNN— -1.0 
ETA - 0.012*RPM/U 
RE - U*L/NU 

CD0 - .00385 + (1.296E-17)*(RE - 1.2E7)**2 

CT - ABS(0.008*L**2*ETA*ABS(ETA)/(A0)) 

Ctl -ABS( 0.008*L**2/(A0)) 

EPS - -1.0+SIGNN/SIGNU* (SQRT(CT+'l. 0)-1.0)/(SQRT( CT1+1.0 )-1.0) 
XPROP - CD0*(ETA*ABS(ETA) - 1.0) 

. .. CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
DO 500 K-1,9 

UCF-(V+X(K)*R)**2+(W-X(K)*Q)**2 

UCF-SQRT(UCF) 

IF (UCF.LT.l.E-6) GO TO 601 

CFLOW -CDY*HH(K)*(V+X(K)*R)**2+CDZ*BR(K)*(W-X(K)*Q)**2 
VECH1(K)-CrLOW*(V+X(X)*R)/UCF 

VECH2(K)-CFLOW*(V+X(K)*R)*X(K)/UCF _ _ 


126 



ooo o n o ono onooon 


VECV1{K)-CFLOW*{W-X{K)*Q)/UCF 
VECV2(K)-CFLOW*(W-X(K)*Q)*X( K)/UCF 
500 CONTINUE 

CALL TRAP{9,VECVl.X,HEAVE) 

CALL TRAP(9,VECV2,X,PITCH) 

CALL TRAP(9,VECH1,X,SWAY ) 

CALL TRAP (9,VECH2,X/YAW ) 

SWAY—0.5*RHO*SWAY 
YAW —0.5*RHO*YAW 
HEAVE—0.5*RHO*HEAVE 
PITCH-+0.5*RHO*PITCH 
GO TO 602 

601 HEAVE-0.0 
PITCH-0.0 
SWAY -0.0 
YAW -0.0 

602 CONTINUE 

FORCE EQUATIONS 


LONGITUDINAL FORCE 

FP(T) - HASS*V*R - MASS*W*Q + MASS*XG*Q**2 + MASS*XG*R**2- 

4 MASS*YG*P*Q - MASS*ZG*P*R + (RHO/2)*L**4*(XPP*P**2 + 

5 XQQ*Q**2 + XRR*R**2 + XPR*P*R) +(RHO/2)*L**3*(XWQ*W*Q + 

6 XVP*V*P+XVR*V*R+U*Q*(XQDS*DS+XQDB*DB)+XRDR*U*R*DR)+ 

6 (RHO/2 ) *L**2* (XW*V**2 + XWW*W**2 + XVDR*U*V*DR + U*W* 

& (XWDS*DS+XWDB*DB)+U**2*(XDSDS*DS**2+XDBDB*DB**2+ 

& XDRDR*DR**2))-(WEIGHT -BOY)*SIN(THETA) +(RHO/2)*L**3* 

i XQDSN*U*Q*DS*EPS+(RHO/2)*L**2*(XWDSN*U*W*DS+XDSDSN*U**2* 

6 DS**2)*EPS +{RHO/2)*L**2*U**2*XPROP 

LATERAL FORCE 

FP(2) - -MASS*U*R - MASS*XG*P*Q + MASS*YG*R**2 - MASS*ZG*Q*R + 

4 (RHO/2)*L**4*(YPQ*P*Q + YQR*Q*R)+(RHO/2)*L**3*(YP*U*P + 

6 YR*U*R + YVQ*V*Q + YWP*W*P + YWR*W*R) + (RHO/2)*L**2* 

& (YV*U*V + YVW*V*W +YDR*U**2*DR) +SWAY +(WEIGHT-BOY)* 

5 COS(THETA)*SIN(PHI)+MASS*W*P+MASS*YG*P**2 

NORMAL FORCE 

FP(3) - KASS*U*Q — MASS*V*P - MASS*XG*P*R - MASS*YG*Q*R + 

6 MASS*ZG*P**2 + MASS*ZG*Q**2 + {RHO/2)*L**4*(ZPP*P**2 + 

& ZPR*P*R + ZRR*R**2) + (RHO/2)*L**3*(ZQ*U*Q + ZVP*V*P + 

6 ZVR*V*R) +,(RHO/2) *L**2*( ZW*U*W + ZW*V**2 + U**2*(ZDS* 

& DS+ZDB*DB))+HEAVE+(WEIGHT-BOY)*COS(THETA)*COS(PHI)+ 

4 (RHO/2)*L**3*ZQN*U*Q*EPS +(RHO/2)*L**2*(ZWN*U*W +ZDSN* 

4 U**2*DS)*EPS 

ROLL FORCE 

FP( 4 ) - -IZ*Q*R +IY*Q*R—IXY*P*R +IYZ*Q**2 -IYZ*R**2 +IXZ*P*Q + 
4 MASS*YG*U*Q -MASS*YG*V*P -MASS*ZG*W*P+(RHO/2)*L**5*(KPQ* 

4 P*Q + KQR*Q*R) +(RHO/2)*L**4*(KP*U*P +KR*U*R + KVQ*V*Q + 

4 KWP*W*P + KWR*W*R) +{RHO/2)*L**3*(KV*U*V + KVW*V*W) + 

4 (YG*WEIGHT - YB*BOY)*COS(THETA)*COS(PHI) - (ZG*WEIGHT - 

4 ZB*BOY)*COS(THETA)*SIN(PHI) + (RHO/2)*L**4*KPN*U*P*EPS+ 

4 (RHO/2)*L**3*U**2*KPROP +MASS*ZG*U*R 
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PITCH FORCE 

FP(5) - -IX*P*R +IZ*P*R +IXY*Q*R -IYZ*P*Q -IXZ*P**2 +IXZ*R**2 - 
fi MASS*XG*U*Q + MASS*XG*V*P + MASS*ZG*V*R - MASS*ZG*W*Q + 

k (RHO/2)*L**5*(MPP*P**2 +MPR*P*R +MRR*R**2)+(RHO/2)*L**4* 

k (MQ*U*Q + MVP*V*P + MVR*V*R) + (RHO/2)*L**3*(MW*U*W + 

k MW*V**2+U>*2*(MDS*DS+MDB*DB) ) + PITCH - (XG*WEIGKT-r 

k XB*BOY)*COS(THETA)*COS(PHI)-t-(RHO/2)*L**4*MQN*U*Q*EPS + 

k (RHO/2)*L**3*(MWN*U*W+MDSN*U**2*DS)*EPS- 

k (ZG*WEIGHT-ZB*BOY)*SIN(THETA) 

YAW rORCE 

FP(6) - -IY*P*Q +IX*P*Q +IXY*P**2 -IXY*Q**2 +IYZ*P*R -IXZ*Q*R - 
k MASS*XG*U*R + MASS*XG*W*P - MASS*YG*V*R + MASS*YG*W*Q + 

& (RHO/2)*L**5*{NPQ*P*Q + NQR*Q*R) +(RHO/2)*L**4*(NP*U*P+ 

k NR*U*R + NVQ*V*Q +NWP*W*P + NWR«W*R) +(RHO/2)*L**3*(NV* 

& U*V + NVW*V*W + NDR*U**2*DR) + YAW + (XG*WEIGHT - 

& XB*BOY)*COS(THETA)*SIN(PHI)+(YG*WEIGHT)*SIN(THETA) 

k + (RHO/2)*L**3*U**2*NPROP-YB*BOY*SIN(THETA) 

NOW COMPUTE THE F(l-6) FUNCTIONS 

DO 600 J - 1,6 

F(J) - 0.0 

DO 600 K - 1,6 

T(J) i XMMINV(J,K)*FP(X) + F(J) 

CONTINUE 

THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 

INERTIAL POSITION RATES F(7-9) 

F(7) - UCO + U*COS(PSI)*C0S{THETA) + V*(COS(PSI)*SIN(THETA)* 
k SIN(PHI) - SIN(PSI)*COS(PHI)) + W*(COS(PSI)*SIN(THETA)* 

& COS(PHI) + SIN(PSI)*SIN(PHI)) 

F(8) - VCO + U*SIN(PSI)*COS(THETA) + V*(SIN(PSI)*SIN{THETA)* 

& SIN(PHI) + COS(PSI)*COS(PHI)) + W*(SIN(PSI)*SIN(THETA)* 

k COS(PHI) - COS(PSI)*SIN{PHI)) 

F(9) - WCO - U*SIN(THETA) +V*COS(THETA)*5IN(PHI) +W*COS(THETA)* 
k COS(PHI) 

EULER ANGLE RATES F(10-12) 

F(10) - P + Q*SIN(PHI)*TAN(THETA) + R*COS(PHI)*TAN(THETA) 

F(11) - Q*COS(PHIj - R*SIN(PHI) 

F(12) - Q*SIN(PHI)/COS(THETA) + R*COS(PHI)/COS(THETA) 


UDOT - F(1) 
VDOT - F{2) 
WDOT - F(3) 
PDOT - F(4) 
QDOT - F(5) 
RDOT - F(6) 
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XDOT - F(7) 

YDOT - F(8) 

ZDOT - T(9) 

PHIDOT - r(10) 

THETAD - F(ll) 

PSIDOT « F(12) 

C 

C ******* CREATE OUTPUT DATA FILE ************************ 

C 

IF (I .EQ. DV ) THEN 
TlMER-FLOAT(I)/2. 

WRITE (20,*) I 

WRITE (20,743) DR/.01745 

WRITE (20,744) XPOS/L,YPOS/L, XD2,YD2 

WRITE (20,746) (PSI-ALPH)/.01745,YLCASE/L 

WRITE (20,746) VCOOBS,UCOOBS 

743 FORMAT .{Ell. 3) 

744 FORMAT ;( 4El2.4) 

746 FORMAT (2E12.4) 

C 

NUMPTS-NUMPTS + 1 
DV“DV+li0/DELT 
END IF 
C 

C***** 

C FIRST ORDER INTEGRATION 

C 



u - 

U + DELT*UDOT 




c 



U 

m 

SURGE RATE 


v - 

V + DELT*VDOT 




c 



V 

m 

SWAY RATE 


W - 

W + DELT*WDOT 




c 



W 

m 

HEAVE RATE 


p - 

P + DELT*PDOT 




c ■ 



P 

m 

ROLL RATE 


Q - 

Q + DELT*QDOT 




c 



Q 

m 

PITCH RATE 


R - 

R + DELT*RDOT 




c 



R 

m 

YAW RATE 


XPOS 

- XPOS + DELT*XDOT 




c 



X 

m 

SURGE 


YPOS 

- YPOS + DELT*YDOT 




c 



Y 

m 

SWAY 


ZPOS 

- ZPOS + DELT*ZDOT 




c 



Z 

m 

HEAVE 


PHI 

- PHI + DELT*PHlDOT 




c 



PHI 

- ROLL 


THETA - THETA + DELT*THETAD 
C THETA - PITCH 

PSI - PSI + DELT*PSiDOT 

C PSI - YAW 

C 

YINTGR-YINTGR + DELT*YLCASE 
C 

C *********SLIDING MODE DEPTH CONTROL************* 

C 

CALL OBSER(QHADOT,THADOT,ZHADOT,QHAT,THAT,ZHAT,DELT,ZPOS,DS,U0) 
C 

S-QHAT + 0.52*THAT - 0.0112*(ZHAT-COMZ*L) 

IF(ABS{S) .LT. BAR) SAT«(S/BAR) 
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IF (S .LE. -BAR) SAT—1.0 

IF»{ S .GE. BAR) SAT-1.0 

UHAT— 5.1429*QHAT + 1.0714*THAT 

UBAR-EITA*SAT 

DE-UHAT+UBAR 

IF (DE .GE. 0.4) DS-0.4 

IE {DE .LE. -0.4) DS—0.4 

IF( (DE .LT. 0.4) .AND. (DE .GT- -0.4)) DS-DE 
DB—DS* 1.0 

********‘SLIDING MODE STEERING CONTROL************ 
******* PLANNER** * ****** 


... . DETERMINE REQUIRED POSITION 

CALL HEAD(L,XPOS, YPOS,XPOS1,YPOS1,ALPH.XLCASE,YLCASE) 

... . DETERMINE IF XLCASE IS WITHIN L/2 DISTANCE OF D 

DAWAY-ABS(XLCASE-XT) 

IF ( DAWAY .LE. 2.0*L ) THEN 

WRITE (*:,*) 'CURRENT POSITION IS ',XPOS/L,YPOS/L,ZPOS/L 

WRITE(*,*) 'SIMULATION TIME IS ',1 

XD1-XD2 

YD1-YD2 

READ (30,*) XD2,YD2,COMZ 

IF ((XD2 .EQ. 0.0) .AND.(YD2 .EQ. 0.0) .AND. 

S (COm .EQ. 0.0) ) GO TO 3 

. CONVERTS LOCAL COORDINATES INTO GLOBAL COORDINATES 

XPOS1-XD1 

YPOSl-YDl 

XPOS2-XD2 

YPOS2-YD2 

. .. DETERMINE THE NEW ANGLE ALPHA FOR THE NEW WAY POINT 

CALL ANGLE( XPOS1, YP0S1-, XPOS2 , YPOS2 , ALPH) 

VC»VCOOBS*COS(ALPK)-UCOOBS*SIN(ALPH) 

UC-UCOOBS*COS(ALPH)-VC00BS*SIN(ALPH) 

.-. , CALCULATE THE LENGTH OF THE NEW PATH 

XT«SQRT((YPOS2-YPOSl)**2 + (XPOS2-XPOS1)* *2) 

XT-XT*L 

. .-. DETERMINE NEW XLCASE AND YLCASE FOR THE NEW WAY POINT 

CALL HEAD(L,XPOS,YPOS,XPOS1,YPOS1,ALPH,XLCASE,YLCASE) 
Z2-VC+S2*YLCASE 

Z3-UC+S3*XLCASE+U*COS(PSI-ALPH) 

. ... RESET YINTGR FOR NEXT WAY POINT 

YINTGR-0.0 

ENDIF 

****** NA vigator******* 
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IF (_(-TIME-TNAV) .GE. 
XA-XID 


YA-YID 

TNAV-TNAV+NAVUPDATE 

ENDIF 


NAVUPDATE ) THEN 1 


****DESIRED SPEED*** 

UD IS SPECIFIED AND HELD CONSTANT 

**»*****RPM INPUT CALCULATION ********* 

SS1-U-UD 

IF(ABS(SSI) .LT. 1.0) SATSGNl-(SSI/1) 

IF ( SSI .LE. -1.0) SATSGNl—1.0 
IF(SSI .GE. 1.0) SATSGN1-1.0 
RPM—1153.9*SATSGN1 + 83.33*U 
IF (RPM .GE. 500.0) RPM- 500.0 
IF (RPM .LE. -500.0) RPM—500.0 ; 

**************************CURRENT OBSERVER******************************* 


ZlDOT-SI*21+(SI *(Al1+A22)/A21+ (A12-A11*A22/A21‘) *U- 
$ SI*Sl/(A21*U))*R+(B1*U-B2*(A11*U-S1)/A21)*U*DR 

Z2DOT»S2*Zl + S2*Z2+S2 *U*SIN(PSI-ALPH)-S2* S2*YLCASE-r 
$ (S2*(A11*U-S1)/(A21*U))*R 

Z3DOT»S3*Z3-S3*S3*XLCASE 
Z1-Z1+Z1D0T*DELT 
Z2“Z2+Z2DOT*DELT 
Z3-Z3+Z3DOT*DELT 
VHAT-Zl+R* (A11’*U-S1 )/(A21*U) 

VCOHAT-Z2-S2*YLCASE 

UCHAT-Z3rS3*XLCASE 

VCOOBS-VCOHAT 

UCOOBS-UCHAT-U*COS(PSI-ALPH) 

* * * * **.* ***************************************************************** 


* * * * * **RUDDER INPUT CALCULATION******* 

DANGLE-(PSI-ALPH) 

IF (DANGLE .GE. 6.2832) THEN 
DANGLE-DANGLE-6.2832 
ENDIF 

VC1-VCOOBS/U 
IF (VCl .GE. 1.0) THEN 
VC-1-1.0 

ELSEIF (VCl .LE. -1.0) THEN 
VCl —1.0 
ENDIF 

SS2»SPl*(DANGLE)+SP2*VHAT+SP3 *R+SP4 *YLCASE+ 

& ((SSPHI*GG15/AKN + SPl)*ASIN(VCl) 

C 

IF(ABS(SS2) .LT. SSPHI ) SATSGN2-(SS2/SSPHI) 

IF(SS2 .LE. SSPHM) SATSGN2—1.0 
IF(SS2 .GE, SSPHI) SATSGN2-1.0 
C 

DR-AKN*SATSGN2+(GG1+(XI)*SP1)*(DANGLE)+(GG2+(XI)*SP2)*VHAT 
S +(GG3+(XI)*SP3)*R+(XI)*SP4 *YLCASE+GG5*YINTGR 

C 

IF (DR .GE. 0.4) DR - 0.4 
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IF (DR .LE. - 0.4) DR - -0.4 
C 

TIME-TIME+DELT 

C 

PHIANG - PHI/0.017-4532925 
THEANG - THETA/0.0174532925 
PSIANG - PSI/0.0174532925 
C 

ALPANG-ALPH/0.0174532925- 
C 

TRAC—YPOS 
ROLL-PHIANG 
YAW-PSIANG 
DEPTH—ZPOS 
PITCH-THEANG 
BOWANG-(DB/:. 01745) 

STNANG-(DS/. 01745 ) 

100 CONTINUE 

3 WRITE(*,*) 'NPTS - ',NUMPTS 

WRITE(*,*) 'TIMEINTERVAL »- ',DELT 
WRITE(*,*) 'NAVIGATOR UPDATE TIME - ',NAVUPDATE 
C WRITE(*,*) 'TARGET RADIUS - '/TARGET 
STOP 
END 

****i**********DEPTH-CONTROL OBSERVER***************** 

SUBROUTINE OBSER(QHADOT,THADOT,ZHADOT,QHAT,THAT,ZHAT,DELT,ZPOS,D 
*S,U)‘ 

QHADOT—0.7*QHAT-0.03*THAT-0.035*DS-20.9293*(ZPOS-ZHAT) 
THADOT-QHAT-14.4092*(ZPOS-ZHAT) 

Z HADOT—6 * THAT+16.45*(ZPOS-ZHAT) 

QHAT- QHAT+DELT*QHADOT 
THAT- THAT+DELT*THADOT 
ZHAT- ZHAT+DELT*ZHADOT 
RETURN 
END 

. ... SUBROUTINE FOR THE ANGLE ALPHA 

SUBROUTINE ANGLE(XI,Y1,X2,Y2,A) 

REAL X1,Y1,X2,Y2,A,DX,DY 

DX-X2-X1 

DY-Y2-Y1 

A-ATAN2(DY,DX) 

RETURN 

END 

. ... SUBROUTINE FOR XLCASE AND YLCASE 

SUBROUTINE HEAD( L,XPOS , YPOS , XP.OS1, YPOS1, ALPH, XLCASE, YLCASE) 

REAL XPOS,YPOS,XPOS1,YPOS1,ALPH,XLCASE,YLCASE,L 

YLCASE-((YPOS-YPOSl*L)*COS(ALPH))-((XPOS-XPOS1*L)*SIN(ALPH)) 

XLCASE-((XPOS-XPOS1*L)*COS(ALPH))+((YPOS-YPOS1*L)*SIN(ALPH)) 

RETURN 

END 

... SUBROUTINE FOR DETERMINING THE REQUIRED POSITION 
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SUBROUTINE POSREQ(TIME,UREQ, XREQ) 

REAL XREQ,UREQ,TIME 

XREQ-UREQ*TIME 

RETURN 

END 

... SUBROUTINE FOR NUMERICAL INTEGRATION USING THE TRAPEZOIDAL RULE 

SUBROUTINE TRAP(N,A,B,OUT) 

DIMENSION A(1),B{1) 

Nl-N-1 
OUT-0.0 
DO 1 1-1,N1 

OUTl-0.5* (A( I )+A(:I+l) ) * ( B( 1 + 1 )-B (I ) ) 

OUT-OUT+OUT1 
1 CONTINUE 
RETURN 
END 
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APPENDIX B 


C ADDED CURRENT AS A DISTURBANCE IN THE CONTROL LAW" 

C ADDED CURRENT OBSERVER 

C ADDED MONITORING OF SECOND LEG TO INITIATE TURN 

Q *************************************************************** 

REAL MASS,LATYAW,NORPIT 

REAL MM{6, 6),G4(4),GK4(4),BR(9) , HH< 9) 

REAL B{6,6),BB(6,6) 

REAL A(12,12 ) , AA( 12,12")=, INDX(100),XDES (100 ) , YDES( 100 ) , ZDES(100) 

REAL XPP ,XQQ ,XRR ,XPR 
REAL XUDOT ,XWQ ,XVP ,XVR 
REAL XQDS ,XQDB , XRDR ,XW 
REAL XWW ,XVDR ,XWDS ,XWDB 
REAL XDSDS,XDBDB ,XDRDR ,XQDSN 
REAL XWDSN ,XDSDSN 

REAL TIME,S,EITA/U3AR,UHAT,COMZ,BAR,SIM,DE,SAT,VHAT,ZZOBS,ZZOBSDOT,SIMl 
REAL SSI,SS2,UD,XD,YD,TD,TNWP,XA,YA,HD,HDMDEG,DAWAY,SATSGN1,SATSGN2 
REAL NAVUPDATE,TNAV,TARGET,FF,GG,HHH,LLL,HDP,HDM,VCUR,UCUR,UCO,VCO,WCO 
REAL UCOOBS,VCOOBS,VCOHAT,UCHAT 
INTEGER by 

LATERAL HYDRODYNAMIC COEFFICIENTS 

REAL YPDOT ,YRDOT,YPQ ,YQR 
REAL YVDOT ,YP ,YR ,YVQ 
REAL YWP ,YWR ,YV ,YVW 
REAL YDR ,CDY 

NORMAL HYDRODYNAMIC COEFFICIENTS 

REAL ZQDOT ,ZPP,ZPR ,ZRR 
REAL ZWDOT ,ZQ ,ZVP ,ZVR 
REAL ZW , 7.W , ZDS ,ZDB 

REAL ZQN ,ZWN ,ZDSN ,CDZ 
REAL ZHADOT,ZHAT 

ROLL HYDRODYNAMIC COEFFICIENTS 

REAL KPDOT ,KRDOT ,KPC ,KQR 
REAL KVDOT , KP ,KR ,KVQ 
REAL KWP , KWR ,KV ,KVW 

REAL KPN , KDB 

PITCH HYDRODYNAMIC COEFFICIENTS 

REAL MQDOT ,MPP ,MPR,MRR 
REAL MWDOT , MQ ,MVP ,MVR 
REAL MW , MW ,MDS , MDB 
REAL MQN , MWN ,MDSN 
REAL QHADOT, QHAT ,_THADOT, THAT 

YAW HYDRODYNAMIC COEFFICIENTS 

REAL NPDOT,NRDOT,NPQ ,NQR 
REAL NVDOT , NP ,NR ,NVQ 

REAL NWP , NWR ,NV ,NVW 
REAL NDR 

MASS CHARACTERISTICS OF THE FLOODED VEHICLE 
REAL WEIGHT , BOY ,VOL ,XG 
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* 


REAL YG ZG ,XB ,ZB 

REAL IX , IY ,IZ ,IXZ 

REAL IYZ , IXY ,YB 

REAL L , RHO ,G ,NU 

REAL AO ,KPROP ,NPROP , XlTEST 

REAL DEGRUD ,DEGSTN 

COMMON /BL0CK1/ F(12), FP(6), XMMINV(6,6), UCF 

INTEGER N, IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 

REAL VECVl(9),VECV2(9),X (12 ) ,VECH1(9),VECH2(9),Xl(9) 

RUDDER COEFFICIENTS 

PARAMETER ( DSMAX- -0.175) 

LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(XPP - 7.E-3 ,XQQ - -1.5E-2 ,XRR - 4.E-3 ,XPR -7.5E-4, 

& XUDOT—7.6E-3 iXWQ - -2.E-1 ,XVP - -3.E-3 ,XVR - 2.E-2, 

& XQDS-2.5E-2 ,XQDB—2.6E-3 ,XRDR- -l.E-3 ,XW -5.3E-2, 

& XWW -1.7E-1 ,XVDR-1.7E-3 ,XWDS«4.6E-2 ,XWDB- l.E-2, 

& XDSDS- -l.E-2 ,XDBDB- -8.E-3 ,XDRDR- -l.E-2 ,XQDSN- 2.E-3, 

& XWDSN-3.5E-3 .XDSDSN- -1.6E-3 ) 

LATERAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(YPDOT-1.2E-4 ,YRDOT-1.2E-3 ,YPQ - 4.E-3 ,YQR —6.5E-3, 

& YVDOT—5.5E-2 ,YP - 3.E-3 ,YR - 3.E-2 ,YVQ -2.4E-2, 

S YWP -2.3E-1 ,YWR —1.9E-2 ,YV - -l.E-1 ,YVW -6.8E-2, 

5 YDR -2.7E-2 /CDY -3.5E-1) 

NORMAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER(ZQDOT—6.8E-3 ,ZPP -1.3E-4 ,ZPR -6.7E-3 ,ZRR —7.4E-3, 

6 ZWDOT—2.4E-1 ,ZQ —1.4E-1 ,ZVP —4.8E-2 ,ZVR -4.5E-2, 

& ZW =. -3.E-1 ,ZW —6.8E-2 ,ZDS —7.3E-2 ,ZDB —2.6E-2, 

& ZQN --2.9E-3 ,ZWN —5.1E-3 ,ZDSN- -l.E-2 ,CDZ - 1.0) 

ROLL HYDRODYNAMIC COEFFICIENTS 

PARAMETER ( KPDOT=* -l.E-3 , KRDOT—3.4E-5 ,KP0 —6.9E-5 , KQR -1.7E-2, 
& KVDOT*l.3E-4 , KP —1.1E-2 ,KR —8.4E-4 ,KVQ—5.1E-3, 

& KWP —1.3E-4 , KWR -1.4E-2 ,KV -3.1E-3 ,KVW —1.9E-1, 

& KPN —5.7E-4 , KDB = 0.0 ) 

PITCH HYDRODYNAMIC COEFFICIENTS 

PARAMETER!MQDOT--1.7E-2 ,MPP -5.3E-5 ,MPR ■ 5.E-3 ,MRR —2.9E-3, 

& MWDOT—6.8E-3 , MQ —6.8E-2 ,MVP -1.2E-3 ,MVR -1.7E-2, 

& MW - l.E-1 , MW —2.6E-2 ,MDS —4.1E-2 ,MDB -6.9E-3, 

& MQN —1.6E-3 , MWN —2.9E-3 ,MDSN —5.2E-3) 

YAW HYDRODYNAMIC COEFFICIENTS 

PARAMETER! NPDOT— 3.4E-5 , NRDOT—3.4 E-3 , NPQ —2.1E-2 , NQR -2.7E-3, 

& NVDOT-1.2E-3 , NP —8.4E-4 ,NR --1.6E-2 ,NVQ = -l.E-2, 

& NWP —1.7E-2 , NWR -7.4E-3 ,NV =-7.4E-3 ,NVW =-2.7E-2, 

& NDR —1.3E-2) 

MASS CHARACTERISTICS OF THE FLOODED VEHICLE 
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PARAMETERf WEIGHT -12000., BOY -12000. ,VOL -200. ,XG - 0. 


& YG - 0.0 

, ZG - 0.20 

,XB - 0. 

,ZB - 0.0 , 

& IX - 1500. 

, IY - 10000. 

,IZ - 10000. 

,IXZ - -10. , 

& IYZ - -10. 

, IXY - -10. 

,YB - 0.0 , 


& L - 17.4 

, RHO - 1.94 

,G - 32.2 

,NU = 8.47E-4 

& A0 - 2.0 

,KPROP - 0. 

jNPROP - 0. , 

X1TEST- 0;.l , 

& DEGRUD- 0.0 

,DEGSTN- 0.0) 



INPUT INITIAL 

CONDITIONS HERE IF 

REQUIRED 



OPEN(20,FILE-'MODELl.DAT',STATUS-'NEW') 


NUMPTS-0.0 
DV-1.0 

*************»*******OBTAIN: INITIAL INFORMATION***************** 


OPEN ( 30,FILE-'INITIAL.DAT',STATUS-'OLD') 

READ (30,*) U0,RPM 

READ (30,*) UD,NAVUPDATE,SIMl,DELT 

... READ IN STEERING AND SLIDING SURFACE GAINS,INITIAL CURRENTS, 
AND SATURATION DESIRED 

OPEN(21,FILE-'SMCINT.DAT',STATUS-'OLD') 

READ(21,*) GG1,GG2,GG3,GG4,GG5 
READ(21,*) SRI,SP2,SP3,SP4,SR5 
READ(21,*) UCO,VCO,WCO 
READ(21,*) AKN,SSPHI 
READ(21,*) IPTS 

... READ IN WAY POINTS 

IF (IPTS .GT. 100) IPTS-100 
DO 200 1=1,IPTS 

READ(30,*) XD,YD,ZD 
XDES(I)«XD*L 
YDES(I)=YD*L 
ZDES(I)=ZD*L 
200 CONTINUE 

PI - 4.0 *ATAN(1.0) 

UC-0.0 
VC-0.0 
UCOOBS-0.0 
VCOOBS-O.0 
VO - 0.0 

wo - o.o 

P0 - 0.0 
Q0 - 0.0 
R0 - 0.0 
PHI0 - 0.0 
THETA0 - 0.0 
PSIO - 0.0 
XPOSO-O.O 
YPOSO-O.0 
ZPOS0-0.0 
XD1-0.0 
YD1-0.0 
Z1-0.Q 
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Z19-0.0 

Z2-0.0 

z 2 a-o.o= 

Z3-0.0 
Z39-0.0 
DB- 0.0 
DS - 0.0 
DR - 0.0 
LATYAW - 0.0 
NORPIT - 0.0 
RE - U0*L/NU 
TNAV-0 
XA-XPOSO 

ya«yposo 

TIME-0.0 
TIMEO-O.O 
U - UO 
V - VO 
W - WO 
P - PO 
Q - QO 
R - RO 

XPOS - XPOSO 

ypos - yposo 

ZPOS - ZPOSO 
PSI - PHIO 
THETA - THETAO 
PHI - PHIO 
QHADOT-O.0 
THADOT-0.0 
ZHADOT-0.0 
QHAT-0.0 
THAT-0.0 
ZHAT-0.0 
VHAT-0.0 
ZOBSDOT-O.0 
ZZOBS - 0.0 

DEFINE THE LENGTH X AND HEIGHT HH TERMS FOR THE DRAG INTEGRATION 

Xl(l) - -105.9/12. 

XI(2) - -99.3/12. 

XI{ 3) - -87.3/12. 

XI(4) - -66.3/12. 

XI(5) - 72.7/12. 

Xl(6) - 83.2/12. 

XI(7) - 91.2/12. 

Xl‘(3) - 99.2/12. 

XI(9) - 103.2/12. 

HH(1) - 0.00/12. 

HH(2) - 8.24/12. 

HH(3) - 19.76/12. 

HH(4) - 29.36/12. 

HH(5) - 31.85/12. 

HH(6) - 27.84/12. 

HH( 7) - 21.44/12. 

HH(8) - 12.00/12. 

HH(9) - 0.00/12. 
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c 

c 


BR( 1) - 

0.00/12.0 

BR( 2 ) - 

8.24/12.0 

BR( 3 ) - 

19.76/12.0 

BR{:4) - 

29.36/12.0 

BR(5) - 

31.85/12.0 

BR ( 6 ) - 

27.84/12.0 

BR( 7) - 

21.44/12.0 

BR (-8.). - 

12.00/12.0 

BR ( 9 J 

0.00/12.0 

MASS - WEIGHT/G 

N ■ 6 

DO 15 J 

- 1, N 


DO 10 K - 1-,N 
XMMINV(J,K) - 0.0 
MM(J, K) - 0.0 
10 CONTINUE 

15 CONTINUE 
C 

MM(1,1) - MASS -<(RHO/2)*(L**3)*XUDOT) 

MM(1,5) - KASS*ZG 
MM(-l’j 6 ) - -MASS*YG 
C 

MM(2,2 ) - MASS -((RHO/2 )*(1**3)* YVDOT) 

MM (2,4) - -MASS*ZG -((RHO/2)*(L**4)*YPDOT) 

MM(2,6) - MASS*XG - ((RHO/2)*(L**4)*YRDOT) 

C 

MM(3,3) - MASS - ((RHO/2)*(L**3)*ZWDOT) 

MM{3,4 ) - MASS*YG 

MM( 3,5 ) - -MASS*XG -{{RHO/2)*(L* *4)*ZQDOT) 

C 

MM{4,2) - -MAS5*ZG - ((RHO/2)*(L**4)*KVDOT) 

MM(4,3) - MASS*YG 

MM(4,4} - IX - ((RHO/2)*(L**5)*KPDOT) 

MM(4,5) - -IXY 

MM(4,6) - -IXZ -((RHO/2)*(L**5)*KRDOT) 

C 

MM(5,1) - MASS*ZG 

MM(5,3) - -MASS*XG -((RHO/2)*(L**4)*MWDOT) 

MM( 5,-4 ) - -IXY 

MM(5 ,S) - IY (RHO/2)*(L**5)*MQDOT) 

MM(5,6) - -IYZ 
C 

MM(6,1) - -MASS*YG 

MM(6,2) - MASS*XG -{{RHO/2)*{D**4)*NVDOT) 

MM(6,4 ) - -IXZ - ((RHO/2)*(L**5)*NPDOT) 

MM(6,5) - -IYZ 

MM(6,6) - IZ - ((RHO/2)*(L**5)*NRDOT) 

C 

C OBSERVER POLES 
C 

SI —1.0 

52— 1.1 

53— 1.2 
C 

C OBSERVER A MATRIX CONSTANTS AND 3 MATRIX CONSTANTS 
C 
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All—0.-04538 
A12—0.35119 
A21—0.002795 
A22—0.09568 
B1 » 0.011432 
B2 —0.004273 
C 

C *****rquTINE FOR INVERTING THE MM MATRIX***** 

C 

DO 12 I-1,N 
DO 11 J-1,N 

XMMINVM,0)-0.0 

11 CONTINUE 
XMMINV(I,I)-1 

12 CONTINUE 

CAul INVTA(MM,N,INDX,D) 

DO 13 J-1,N 

CALL INVTB(MM,N,INDX,XMMINV( 1, J)) 

13 CONTINUE 
C 

C ************ INPUTS ************* 

C 

C RUDDER AND DIVE PLANE COMMANDS 

C 

SIM-SIMl/DELT 
TIME-0.0 
DS- 0.0 
DR- 0.0 
DB- 0.0 
EITA-4.0 
BAR-.4 
C 

YINTGR-0.0 
SSPHM—SSPHI 
C • 

ISIM-SIMl/DELT 
ISTART-1 
C 

C ***************»***siMULATION BEGINS **************** 

C 

C LOOP OVER WAY POINTS 
C 

DO 210 IP-1,IPTS 

IF (-IP .GE. 2) GO TO 211 
XD-XDES(1) 

YD-YDES(l) 

XD1-0.0 
YD1-0.0 
XD2-XD 
YD2^YD 
GO TO 212 

211 XD-XDES(IP) 

YD-YDES(IP) 

XD1-XD2 

YD1-YD2 

XD2-XD 

YD2-YD 

212 YDl2-(YD2-YD1) 

XD12-(XD2-XD1) 

ALPH-ATAN{YD12/XD12) 
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ALPH-ABS(ALPH) 

IF ((XDl'2'.GE. 0.0) .AND. (YD12 .GE. 0.0)) ALPH- ALPH 

IF ({XD12 .GE. 0.0) .AND. (YD12 .LT. 0.0)) ALPH- -ALPH 

IF ((XD12 .LT. 0.0) .AND. (YD12 .GE. 0.0) ) ALPH-PI-ALPH 

IF ( (XD12 .LT. 0.0) .AND. (YD12 .LT. 0.0)1) ALPH-PI+ALPH 

VCHAT«VC*COS(ALPH)-UC*SIN(ALPH) 

UCHAT-VC*SIN(ALPH)+UC*COS(ALPH) 

YCTE-(YPOS-YD1)*COS(ALPH)-(XPOS-XD1)*SIN(ALPH) 

XCTE-(YPOS-YD1)*SIN(ALPH)+(XPOS-XD1)*COS(ALPH) 

Z2-VCHAT+S2*YCTE 

Z3-UCHAT+S3 *XCTE+U*COS(PSI-ALPH) 

4 WRITE(*,101) XD/L,YD/L 

WRITE!*,102 ) XD12/L,YD12/L,ALPH*(180/PI1, I START 

101 FORMAT(' HEADING FOR (X,Y) - (',F9.3,',',F9.3,')') 

102 FORMAT!' XD12- ',F8.3,' YD12- ',F8.3,' ALPH- ',F9.3,' ISTART- 

4 16) 

DO 100 M-ISTART,ISIM 
ICOUNT-M 

PROPULSION MODEL 
SIGNU - 1.0 

IF (U.LT.0.0) SIGNU - -1.0 
IF (ABS(U).LT.X1TEST) U - X1TEST 
SIGNN - 1.0 

IF (RPM.LT.0.0) SIGNN - -1.0 
ETA - 0.012*RPM/U 
RE - U*L/NU 

CD0 - .00385 + (1.296E-17.) * (RE - 1.2E7)**2 

CT - ABS(0.008*L**2*ETA*ABS(ETA)/(A0)) 

CT1 -ABS( 0.008*L**2/(A0)) 

EPS - -1.0+SIGNN/SIGNU*(SQRT{CT+1.0)-1.0)/(SQRT(CT1 + 1.0)-1.0) 
XPROP - CD0*(ETA*ABS(ETA) - 1.0) 

... CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
DO 500 K-1,9 

UCF-(V+X(K)*R)**2+(W-X(K)*Q)**2 

UCF-SQRT(UCF) 

IF (UCF.LT.l.E-6) GO TO 601 

CFLOW «CDY*HH(K)*(V+X(K)*R)**2+CDZ*BR(K)*(W-X(K)*Q)**2 
VECH1(K)-CFLOW*(V+X(K)*R)/UCF 
VECH2(K)-CFLOW*(V+X{K)*R)*X(K)/UCF 
VECVl(K)-CFLOW*(W-X{K)*Q)/UCF 
VECV2(K)-CFLOW*(W-X(K)*Q)*X(K)/UCF 
500 CONTINUE 

CALL TRAP(9,VECVl,X,HEAVE) 

CALL TRAP(9,VECV2,X,PITCH) 

CALL TRAP(9,VECH1,X,SWAY ) 

CALL TRAP! 9,VECH2,X,YAW ) 

SWAY—0.5*RHO*SWAY 
YAW —0.5*RHO*YAW 
HEAVE—0. S*RHO*HEAVE 
PITCH-+0.5*RHO*PITCH 
GO TO 602 
601 HEAVE-0.0 
PITCH-0.0 
SWAY -0.0 
YAW -0.0 
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602 CONTINUE 

FORCE EQUATIONS 


LONGITUDINAL FORCE 

FP(1) - MASS*V*R - MASS*W*Q + MASS*XG*Q**2 + MASS*XG*R**2- 
& MASS *YG*P*Q - MASS*ZG*P*R + (RHO/2)*L**4*(XPP*P**2 + 

& XQQ*Q**2 + XRR*R* * 2 + XPR*P*R) +(RHO/2)*L**3*(XWQ*W*Q + 

& _XVP*V*P+XVR*V*R+U*Q*(XQDS*DS+XQDB*DB)+XRDR*U*R*DR)+ 

& (RHO/2)*L**2*{XVV*V* *2 + XWW*W**2 + XVDR*U*V*DR + U*W* 

& (XWDS*DS+XWDB*DB)+U**2*(XDSDS*DS**2+XDBDB*DB**2+ 

£ XDRDR*DR**2))-(WEIGHT -BOY)*SIN(THETA) +(RHO/2)*L**3* 

£ XQDSN*U*Q*DS*EPS+(RHO/2)*L* *2*(XWDSN*U*W*DS+XDSDSN*U* * 2* 

£ DS**2)*EPS +(RHO/2)*L**2*U**2*XPROP~ 

LATERAL FORCE 

FP(2) - -MASS*U*R - MASS*XG*P*Q + MASS*YG*R**2 - MASS*ZG*Q*R + 

& (RHO/2)*L* *4*(YPQ*P*Q + YQR*Q*R)+ (RHO/2)*L**3* (YP»U*P + 

£ YR*U*R + YVQ*V*Q + YWP*W*P + YWR*W*R) + (RHO/2)*L**2* 

5 (YV*U*V + YVW*V*W +YDR*U* * 2 *DR) +SWAY +(WEIGHT-BOY)* 

6 COS(THETA)*SIN(PHI)+MASS*W*P+MASS*YG*P**2 

NORMAL FORCE 

FP( 3) - MASS*U*Q - MASS*V*P - MASS*XG*P*R - MASS*YG*Q*R + 

£ MASS*ZG*P**2 + NASS*ZG*Q**2 + (RHO/2)*L**4*(ZPP*P»*2 + 

& ZPR*P«R + ZRR*R**2) + (RHO/2)*L**3*(ZQ*U*Q + ZVP*V*P + 

5 ZVR*V*R) +(RHO/2 )*L**2*(ZW*U*W + ZW*V*»2 + U**2*(ZDS* 

£ DS+ZDB*DB))+HEAVE+(WEIGHT-BOY)*COS(THETA)*COS(PHI)+ 

£ (RHO/2)*L**3*ZQN*U*Q*EPS +(RHO/2)*L**2*(ZWN*U*W +ZDSN* 

£ U**2*DS)*EPS 

ROLL FORCE 

FP(4) » -IZ*Q*R +IY*Q*R -IXY*P*R +IYZ*Q*«2 -IYZ*R**2 +IXZ*P*Q + 
£ MASS *YG*U*Q -MASS»YG*V*P -MASS*ZG*W*P+(RHO/2)*L* *5* ( KPQ* 

£ P*Q + KQR*Q*R) +{RHO/2)*L**4 *(KP*U*P +KR*U*R + KVQ*V*Q + 

£ KWP*W*P + KWR*W*R) +(RHO/2)*L*»3*(KV*U*V + KVW*V*W) + 

£ (YG*WEIGHT - YB*BOY)*COS(THETA)*COS(PHI) r (ZG»WEIGHT - 

£ ZB*BOY)*COS(TH£TA)*SIN(PHI) + (RHO/2)*L**4*KPN»U*P*EPS-r 

£ (RHO/2)*L**3*U**2*KPROP +MASS*ZG*U*R 

PITCH FORCE 

FP(5) - -IX*P*R +IZ*P*R +IXY*Q*R -IYZ*P*Q -IXZ*P**2 +IXZ*R**2 - 
£ MASS*XG*U*Q + MASS*XG*V*P + MASS*ZG*V*R - MASS*ZG*W*Q + 

£ {RHO/2)*L**5*(MPP*P**2 +MPR*P*R +MRR»R*»2) + (RHO/2)*L**4 * 

£ (MQ*U*Q + MVP*V*P + MVR*V*R) + (RHO/2)*L**3*( MW*U*W + 

£ MW*V**2+U**2*(MDS*DS+MDB*DB))+ PITCH -(XG*KEIGHT- 

£ XB*BOY)*COS(THETA)*COS(PHI)+{RHO/2) *L* *4*MQN*U*Q*EPS + 

£ (RHO/2)*L**3*(MWN*U*W+MDSN*U**2*DS)*EPS- 

£ (ZG*WEIGHT-Z3*BOY)*SIN(THETA) 

YAW FORCE 

FP(6) - -IY*P*Q +IX*P*Q +IXY*?**2 -IXY*Q**2 +IYZ*P*R -IXZ»Q*P. - 
£ MASS*XG*U*R + MASS*XG*W*P - MASS*YG*V*R + MASS»YG»W»Q + 
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& (RHO/2)*L**5*(NPQ*P*Q + NQR*Q*R) +(RHO/2)*L**4*(NP*U*P+ 

& NR*U*R + NVQ*V*Q +NWP*W*P + NWR*W*R) +{RHO/2)*L**3*(NV* 

& U*V + NVW*V*W + NDR*U**2*DR) + YAW + (XG*WEIGHT - 

& XB*BOY)*COS(THETA)*SIN(PHI)+(YG*WEIGHT)*SIN(THETA) 

& +(RHO/2)*L**3*U**2*NPROP-YB*BOY*SIN(THETA) 


NOW COMPUTE THE F(l-6) FUNCTIONS 

DO 600 J - 1,6 

F(J) - 0.0 
DO 600 K - 1,6 

F(J) - XMMINVtJ,K)*FP(K) + F(J) 

CONTINUE 

THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 
INERTIAL POSITION RATES F(7-9) 

F( 7) - UCO + U*COS(PSI) *COS(THETA) + V*(COS(PSI)*SIN(THETA)* 

& SIN(PHI) - SIN(PSI)*COS(PHI)) + W*{COS(PSI)»SIN{THETA)* 

4 COS{PHI) + SIN(PSI)*SIN(PHI)) 

F( 8) - VCO + U*SIN(PSI)*COS(THETA) + V*{SIN(PSD *SIN(THETA)* 

& SIN(PHI) + COS(PSD *COS(PHI) ) + W*(SIN(PSI)*SIN(THETA)* 

& COS(PHI) - COS{PSI)*SIN(PHI)) 

F(9) - WCO - U*SIN{THETA) +V*COS(THETA)*SIN(PHI) +W*COS(THETA)* 
& COStPHI) 

EULER ANGLE RATES Ft 10-12) 

Ft 10) - P + Q*SIN(PHI)*TAN(TKETA) + R*COS(PHI ) *TAN(THETA) 

F(11) - Q*COS(PHI) - R*SIN(PHI) 

Ft 12) « Q*SIN(PHI)/COS(THETA) + R*COS(PHI)/COS(THETA) 


UDOT - F(1) 

VDOT - F{2) 

WDOT - Ft 3) 

PDOT - F(4) 

QDOT - F{5) 

ROOT - F(6) 

XDOT - F(7) 

YDOT - Ft 8) 

ZDOT - F(9) 

PHIDOT - F(10) 

THETAD - F(11) 

PSIDOT - Ft 12) 

CREATE OUTPUT DATA FILE 

IF (M .EQ. DV ) THEN 
TIMER-FLOAT(M)/2. 

WRITE (20,*) M 

IF (DR .GT. 0.4) THEN 
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DR *0.4 

ELSEIF (DR . LT. -0.4) THEN 
DR - -0.4 
ENDIF 
C 

WRITE (20,743) DR/.01745 

WRITE (20,744) XPOS/L,YPOS/L,XDES(I?)/L,YDES(IPJ/L 
WRITE (20,746) (PSI-ALPH)/.01745,YCTE/L 
C 

IF (DR99 .GT. 0.4) THEN 
DR99 - 0.4 

ELSEIF (DR99 .LT. -0.4) THEN 
DR99 - -0.4 
ENDIF 
C 

WRITE (20,744) YCTE99/L,DR99/0.01745,SS99,PROD 

743 FORMAT (Ell.3) 

744 FORMAT (4E12.4) 

746 FORMAT (2E12.4) 

C 

NUMPTS-NUMPTS + 1 

DV-DV-*1.0/DELT 

ENDIF 

***** 


FIRST ORDER INTEGRATION 


U - U + DELT*UDOT 

U 

- SURGE RATE 

V * V + DELT*VDOT 

V 

- SWAY RATE 

W « W + DELT*WD0T 

W 

- HEAVE RATE 

P * P + DELT»PDOT 

? 

- ROLL RATE 

Q - Q + DELT«QDOT 

Q 

- PITCH RATE 

R - R + DELT*RDOT 

R 

- YAW RATE 

XPOS - XPOS + DELT»XDOT 

X 

- SURGE 

YPOS - YPOS + DELT*YDOT 

Y 

- SWAY 

EPOS - ZPOS + DELT*ZDOT 

Z 

- HEAVE 

PHI - PHI +- DELT*PHIDOT 

PHI - ROLL 

THETA - THETA DELT*TKETAD 

THETA - PITCH 

PSI - PSI + DELT*PSIDOT 

PS 

I - YAK 

YINTGR-YINTGR + DELT*YLCASE 

INTEGRAL OF LATERAL 

DEVIATION ERROR 


*.*...... SLI Di;;g MODE DEPTH CONTROL***. 

CALL OBSER(QHnDOT.THADOT,ZKADOT,QKAT,TKA7,ZHAT,DELT,ZPOS,DS.U0J 
C 

S-QHAT * 0.52‘THAT - 0.0112*(ZHAT-CCMZ-L) 

IFIABSIS) .LT. BAR) SAT-(S/BAR) 
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IF( S .LE. -BAR) SAT—1.0 

IF(S .GE. BAR) SAT-1.0 

UHAT--5.1429*QHAT + 1.0714*THAT 

UBAR-EITA*SAT 

DE-UHAT+UBAR 

IF (DE .GE. 0.4) DS-0.4 

IF (DE .LE. -0.4) DS —0.4 

IF( (DE .LT. 0.4) .AND. (DE .GT. -0.4)) DS-DE 
DB—DS*1.0 

*********SLIDING MODE STEERING CONTROL ************ 


*******planner********* 


. .. DETERMINE REQUIRED POSITION 

YCTE- (YPOS-YD1.) *COS (ALPH) - (XPOS-XDl)* SIN(ALPH) 

XCTE- (YPOS-YDl;) *SIN(ALPH) + (XPOS-XDl) *COS (ALPH) 

. .. DETERMINE IF XLCASE IS WITHIN L/2 DISTANCE OF D 

XT-SQRT((XD2-XD1)**2 + (YD2-YD1) * * 2 ) 

XAWAY-(XT-XCTE) 

DAWAY-ABS(XAWAY) 

************************ * *CURRENT OBSERVER******************************* 


2lDOT-Sl*Zl+(Sl*(A11+A22)/A21+(Al2-All*A22/A21)*U- 
$ S1*S1/(A21*U))*R+(B1*U-B2*(A11*U-S1)/A21)*U*DR * 

22DOT“S2*21+S2*22+S2*U*SIN(PSI-ALPH)-S2*S2*YCTE+ 

§ (S2*(All*U-Sl)/(A21*U))*R 

Z3D0T»S3*Z3-S3*S3*XCTE 

Zl-21fZlDOT*DELT l 

22-Z2+Z2DOT*DELT 

Z3«Z3+Z3DOT*DELT 

VHAT-21+R*(All*U-Sl)/(A21*U) 

VCHAT-Z2-S2*YCTE 

UCHAT»Z3-S3*XCTE 

VCOOBS-VCHAT 

UCOOBS-UCHAT-U*COS(PSI-ALPH) 

VC-UCOOBS*SIN(ALPH)+VCOOBS*COS(ALPH) 

UC-UCOOBS *COS(ALPH)-VCOOBS * SIN(ALPH) 

IF (IP .LT. IPTS) GO TO 250 
IF (DAWAY .LT. 0.1) GO TO 201 
GO TO 230 

MONITOR CONTROL LAW FOR NEXT SEGMENT 


250 YN12-YDES(IP+1)-YDES(IP) 
XN12-XDES(IP+1)-XDES(IP) 

B ETA-ATAN(YN12/XN12) 
BETA-ABS(BETA) 

IF ((XN12 .GE. 0.0) .AND. 
IF ((XN12 .GE. 0.0) .AND. 
IF ((XN12 .LT. 0.0) .AND. 
IF ((XN12 .LT. 0.0) .AND. 


(YN12 .GE. 0.0)) BETA- BETA 
(YN12 .LT. 0.0J) BETA- -BETA 
(YN12 .GE. 0.0)) BETA-PI-BETA 
(YN12 .LT. 0.0)) BETA-PI+BETA 


... CURRENT OBSERVER FOR NEXT PATH 
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YCTE99-{YPOS-YDES(IP))*COS(BETA)-(XPOS-XDES(IP))*SIN(BETA) 
VC99-VC*COS(BETA)-UC*SIN(BETA) 

VC2-VC99/U 

IF (VC2 .GE. 1.0) THEN 
VC2-1.0 

ELSEIF (VC2 .LE. -1.0) THEN 
VC2 —1.0 
ENDIF 
C 

SS99-SPl*(PSI-BETA)+SP2*VHAT+SP3 *R+SP4 *YCTE99 + 

? ((SSPHI*GG1)/AKN+SP1)*ASIN(VC2) 

IF (ABS(SS99) .LT. SSPHI) SPHI99-SS99/SSPHI 
IF (SS99 .LE. SSPHM) SPHI99—1.0 
IF (SS99 .GE. SSPHI) SPHI99- 1.0 
DR98-GG1*(PSI-BETA)+GG2*VHAT+GG3 *R+GG4 *YCTE99 
DR97-AKN*SPHI99 
DR99-DR97+DR98 
IF (DR99 .GE. 0.4) DR99-0.4 
IF (DR99 .:LE. -0.4) DR99—0.4 
DRNEW-DR99 
C 

IF (M .EQ. 1) GO TO 230 
PROD-DROLD*DRNEW 
C 

IF (XAWAY .LE. 0.0) GO TO 201 
IF (XAWAY .GT. (,0.5*XT)) GO TO 230 
IF (PROD .LE. 0.0) GO TO 201 
C 

230 DROLD-DRNEW 

*****♦NAVIGATOR"****** 

IF ((TIME-TNAV) .GE. NAVUPDATE ) THEN 
XA-XID 
YA-YID 

TNAV-TNAV+NAVUPDATE 
ENDIF 

***‘HEADING********* 

****DESIRED SPEED*** 

UD IS SPECIFIED AND HELD CONSTANT 

*****«**RPM INPUT CALCULATION ********* 

SS1-U-UD 

IF(ABS(SSI) .LT. 1.0) SATSGN1”(SS1/SSPHI) 

I F( SSI .LE. SSPHM) SATSGN1>*-1.0 
IF(SSI .GE. SSPHI) SATSGN1- 1.0 
RPM—1153.9*SATSGN1 + 83.33*U 
IF (RPM .GE. 500.0) RPM- 500.0 
IF (RPM .LE. -500.0) RPM—500.0 

*******rUDDER INPUT CALCULATION******* 

********«***BEGIN SMC CALCULATIONS ********************* 

DANGLE-(PSI-ALPH) 

VC1-VCOOBS/U 
IF (VC1 .GE. 1.0) THEN 
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•ELSEIF (VC1 .LE. -1.0) THEN 
VC1 —1.0 
END IF 
C 

SS2-SP1*(DANGLE)+SP2 *VHAT+SP3 *R+SP4 *YCTE+ 

& ((SSPHI*GG1)/AKN + SP1)*ASIN(VC1) 

C 

IF(ABS(SS2) .LT. SSPHI) SATSGN2-(SS2/SSPHI) 

IF(SS2 .LE. SSPHM) SATSGN2—T.O 
. IF(SS2 .GE. SSPHI) SATSGN2-1.0 

C 

DR«AKN*SATSGN2+GG1*(DANGLE)+GG2*VHAT+GG3*R+GG4*YCTE+GG5*YINTGR 
C 

IF (DR .GE. 0.4) DR - 0.4 

IF (DR .LE. - 0.4) DR - -0.4 
C 

TIME-TIME+DELT 

C 

PHIANG - PHI/0.0174532925 
THEANG - THETA/0.0174532925 
PSIANG - PSI/O.0174532925 
C 

ALPANG-ALPH/O.0174532925 
C 

TRAC—YPOS 

ROLL-PHIANG 

YAW-PSIANG 

DEPTH—ZPOS * 

PITCH-THEANG 

BOWANG»(DB/.01745) 

STNANG-(DS/.01745) 

100 CONTINUE 
GO TO 300 

201 ISTART”ICOUNT 

WRITE(*,103) YCTE99 

103 FORMAT(' YCTE99- ',F9.3) 

WRITE(*,104) SS99 

104 FORMAT(' SS99- ',F9.3) 

WRITE(*,105) DR99 

105 FORMAT(' DR99- ',F9.3) 

WRITE( *,106): DRNEW 

106 FORMAT(' DRNEW- ',F9.3) 

WRITE( *,107), DROLD 

107 FORMAT (' DROLD- ',F9.3) 

WRITE(*,108) PROD 

108 FORMAT!' PROD- ',F9.3) 

210 CONTINUE 

C 400 CONTINUE 

300 WRiTE(*,*) 'NPTS - ',NUMPTS 

WRITE(*,*) 'TIMEINTERVAL - ',DELT 
WRITE(*,*) 'NAVIGATOR UPDATE TIME - ',NAVUPDATE 
C WRITE!*,*) 'TARGET RADIUS - ',TARGET 

STOP 

END c 

*************** DEPTH CONTROL OBSERVER* * *************** 

SUBROUTINE OBSER(QHADOT,THADOT,ZHADOT,QHAT,THAT,ZHAT,DELT,ZPOS,D 
*S,U) 
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QHADOT--Q.7*QHAT-0.03*THAT-0.035*DS-20.9293*(ZPOS-ZHAT} 
THADOT-QHAT-14.4092*(ZPOS-ZHAT) 

ZHADOTi— 6 *THAT+16.4 5 *(ZPOS-ZHAT) 

C 

QHAT- QHAT+DELT*QHADOT 
THAT- THAT+DELT*THADOT 
ZHAT- ZHAT+DELT*ZHADOT 
RETURN 
END 

... SUBROUTINE FOR THE ANGLE ALPHA 

SUBROUTINE ANGLE(XI,Y1,X2,Y2,A) 

REAL Xl,Y1,X2,Y2,A,DX,DY 

DX-X2-X1 

DV-Y2-yl 

A-ATAN2(DY,DX) 

RETURN 

END 

... SUBROUTINE FOR XLCASE AND YLCASE 

SUBROUTINE HEAD(L,XPOS,YPOS,XPOSI,YPOS1,ALPH,XLCASE,YLCASE) 

REAL XPOS,YPOS,XPOSI,YPOS1,ALPH,XLCASE,YLCASE,L 

YLCASE-({YPOS-YPOSl*L)*COS(ALPH))-((XPOS-XPOSI*L)*SIN(AL?H)) 

XLCASE-((XPOS-XPOSI* L)*COS(ALPH)) + (-(YPOS-YPOS1* L)*SIN(ALPH)) 

RETURN 

END 

... SUBROUTINE FOR NUMERICAL INTEGRATION USING THE TRAPEZOIDAL RULE 

SUBROUTINE TRAP(N,A,B,OUT) 

DIMENSION A(1),B(1) 

Nl-N-1 
OUT-0.0 
DO 1 1-1,N1 

OUTl-O.5*(A(I)+A(I+1))*(B(I+1)-B(I)) 

OUT-OUT+OUT1 
1 CONTINUE 
RETURN 
END 
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